public SwerveDrive(CANTalon fr, CANTalon fra, AnalogInput fre, CANTalon fl, CANTalon fla, AnalogInput fle, CANTalon br, CANTalon bra, AnalogInput bre, CANTalon bl, CANTalon bla, AnalogInput ble) { FRM = new SwerveModule(fr, fra, fre, "FR"); FLM = new SwerveModule(fl, fla, fle, "FL"); BRM = new SwerveModule(br, bra, bre, "BR"); BLM = new SwerveModule(bl, bla, ble, "BL"); length = Config.getSetting("FrameLength",1); width = Config.getSetting("FrameWidth",1); diameter = Math.sqrt(Math.pow(length,2)+Math.pow(width,2)); temp = 0.0; a = 0.0;b = 0.0;c = 0.0;d = 0.0; ws1 = 0.0;ws2 = 0.0;ws3 = 0.0;ws4 = 0.0; wa1 = 0.0;wa2 = 0.0;wa3 = 0.0;wa4 = 0.0; max = 0.0; movecount = 0; }
public static void init() { driveTrainCIMRearLeft = new CANTalon(2); // rear left motor driveTrainCIMFrontLeft = new CANTalon(12); // driveTrainCIMRearRight = new CANTalon(1); driveTrainCIMFrontRight = new CANTalon(11); driveTrainRobotDrive41 = new RobotDrive(driveTrainCIMRearLeft, driveTrainCIMFrontLeft, driveTrainCIMRearRight, driveTrainCIMFrontRight); climberClimber = new CANTalon(3); c1 = new Talon(5); pDPPowerDistributionPanel1 = new PowerDistributionPanel(0); gyro = new ADXRS450_Gyro(Port.kOnboardCS0); c2 = new Talon(6); ultra = new AnalogInput(0); LiveWindow.addSensor("PDP", "PowerDistributionPanel 1", pDPPowerDistributionPanel1); LiveWindow.addSensor("Ultra", "Ultra", ultra); // LiveWindow.addActuator("Intake", "Intake", intakeIntake); LiveWindow.addActuator("Climber", "Climber", climberClimber); LiveWindow.addActuator("RearLeft (2)", "Drivetrain", driveTrainCIMRearLeft); LiveWindow.addActuator("FrontLeft (12)", "Drivetrain", driveTrainCIMFrontLeft); LiveWindow.addActuator("RearRight (1)", "Drivetrain", driveTrainCIMRearRight); LiveWindow.addActuator("FrontRight (11)", "Drivetrain", driveTrainCIMFrontRight); LiveWindow.addActuator("Drive Train", "Gyro", gyro); // LiveWindow.addActuator("Shooter", "Shooter", shooter1); }
public SpatialAwarenessSubsystem() { super("SpatialAwarenessSubsystem"); cameraServer = CameraServer.getInstance(); gearCamera = cameraServer.startAutomaticCapture(0); gearCamera.setResolution(IMG_WIDTH, IMG_HEIGHT); gearCamera.setFPS(30); gearCamera.setBrightness(7); gearCamera.setExposureManual(30); gearVideo = cameraServer.getVideo(gearCamera); visionProcessing = new VisionProcessing(); leftUltrasonic = new AnalogInput(RobotMap.ANALOG_ULTRASONIC_LEFT); rightUltrasonic = new AnalogInput(RobotMap.ANALOG_ULTRASONIC_RIGHT); leftUltrasonicKalman = new SimpleKalman(1.4d, 64d, leftUltrasonic.getAverageVoltage() * ULTRASONIC_VOLTS_TO_METERS); rightUltrasonicKalman = new SimpleKalman(1.4d, 64d, rightUltrasonic.getAverageVoltage() * ULTRASONIC_VOLTS_TO_METERS); navX = new AHRS(SPI.Port.kMXP); System.out.println("SpatialAwarenessSubsystem constructor finished"); }
/** * Initialize the gyro. Calibration is handled by calibrate(). */ public void initGyro() { result = new AccumulatorResult(); m_voltsPerDegreePerSecond = kDefaultVoltsPerDegreePerSecond; m_analog.setAverageBits(kAverageBits); m_analog.setOversampleBits(kOversampleBits); double sampleRate = kSamplesPerSecond * (1 << (kAverageBits + kOversampleBits)); AnalogInput.setGlobalSampleRate(sampleRate); Timer.delay(0.1); setDeadband(0.0); setPIDSourceType(PIDSourceType.kDisplacement); UsageReporting.report(tResourceType.kResourceType_Gyro, m_analog.getChannel()); LiveWindow.addSensor("AnalogGyro", m_analog.getChannel(), this); }
/** * {@inheritDoc} */ public double getAngle() { if (m_analog == null) { return 0.0; } else { m_analog.getAccumulatorOutput(result); long value = result.value - (long) (result.count * m_offset); double scaledValue = value * 1e-9 * m_analog.getLSBWeight() * (1 << m_analog.getAverageBits()) / (AnalogInput.getGlobalSampleRate() * m_voltsPerDegreePerSecond); return scaledValue; } }
/** * Gyro constructor with a precreated analog channel object. Use this * constructor when the analog channel needs to be shared. * * @param channel The AnalogInput object that the gyro is connected to. * Analog gyros can only be used on on-board channels 0-1. */ public AnalogGyro(AnalogInput channel) { analogInput = channel; if (analogInput == null) { throw new NullPointerException("AnalogInput supplied to AnalogGyro constructor is null"); } result = new AccumulatorResult(); analogInput.setAverageBits(DEFAULT_AVERAGE_BITS); analogInput.setOversampleBits(DEFAULT_OVERSAMPLE_BITS); updateSampleRate(); analogInput.initAccumulator(); UsageReporting.report(tResourceType.kResourceType_Gyro, analogInput.getChannel(), 0, "Custom more flexible implementation"); LiveWindow.addSensor("Analog Gyro", analogInput.getChannel(), this); calibrate(DEFAULT_CALIBRATION_TIME); }
/** * Return the actual angle in degrees that the robot is currently facing. * * The angle is based on the current accumulator value corrected by the * over-sampling rate, the gyro type and the A/D calibration values. The * angle is continuous, that is it will continue from 360 to 361 degrees. * This allows algorithms that wouldn't want to see a discontinuity in the * gyro output as it sweeps past from 360 to 0 on the second time around. * * @return the current heading of the robot in degrees. This heading is * based on integration of the returned rate from the gyro. */ public double getAngle() { if (m_analog == null) { return 0.0; } else { m_analog.getAccumulatorOutput(result); // correct the accumulated value by subtracting the offset // Note: The result.count is after the center subtraction long value = result.value - (long) (result.count * m_offset); // compute the angle double scaledValue = value * 1e-9 * m_analog.getLSBWeight() * (1 << m_analog.getAverageBits()) / (AnalogInput.getGlobalSampleRate() * m_voltsPerDegreePerSecond); // return the angle return scaledValue; } }
/** * * @param valueToInches voltage to inches * @param port analogue port */ public Ultrasonic(double valueToInches, int port){ valueToInches = this.valueToInches; port = this.port; ultrasonic = new AnalogInput(port); lastLocation = ultrasonic.getValue() * valueToInches; }
public SwerveModule(CANTalon d, CANTalon a, AnalogInput e, String name) { SpeedP = Config.getSetting("speedP",1); SpeedI = Config.getSetting("speedI",0); SpeedD = Config.getSetting("speedD",0); SteerP = Config.getSetting("steerP",2); SteerI = Config.getSetting("steerI",0); SteerD = Config.getSetting("steerD",0); SteerTolerance = Config.getSetting("SteeringTolerance", .25); SteerSpeed = Config.getSetting("SteerSpeed", 1); SteerEncMax = Config.getSetting("SteerEncMax",4.792); SteerEncMax = Config.getSetting("SteerEncMin",0.01); SteerOffset = Config.getSetting("SteerEncOffset",0); MaxRPM = Config.getSetting("driveCIMmaxRPM", 4200); drive = d; drive.setPID(SpeedP, SpeedI, SpeedD); drive.setFeedbackDevice(FeedbackDevice.QuadEncoder); drive.configEncoderCodesPerRev(20); drive.enable(); angle = a; encoder = e; encFake = new FakePIDSource(SteerOffset,SteerEncMin,SteerEncMax); PIDc = new PIDController(SteerP,SteerI,SteerD,encFake,angle); PIDc.disable(); PIDc.setContinuous(true); PIDc.setInputRange(SteerEncMin,SteerEncMax); PIDc.setOutputRange(-SteerSpeed,SteerSpeed); PIDc.setPercentTolerance(SteerTolerance); PIDc.setSetpoint(2.4); PIDc.enable(); s = name; }
/** * Creates the GearManipulator, and sets servo to closed position. */ public GearManipulator() { logger.info("Initialize"); gearServo = new Servo(RobotMap.GEAR_SERVO_CHANNEL); gearServo.setBounds(2.4, 0, 0, 0, 0.8); gearServo.setPeriodMultiplier(PeriodMultiplier.k4X); setPosition(Position.CLOSED); lsSpokeDown = new AnalogInput(RobotMap.AI_LS_SPOKE_DOWN); lsWedgeDown = new AnalogInput(RobotMap.AI_LS_WEDGE_DOWN); pressurePlate = new AnalogInput(RobotMap.AI_PRESSURE_PLATE); }
/** * Default constructor. * * @param port The port of the sensor. * @param oversampleBits The number of oversample bits. * @param averageBits The number of averaging bits. */ @JsonCreator public PressureSensor(@JsonProperty(required = true) int port, @JsonProperty(required = true) int oversampleBits, @JsonProperty(required = true) int averageBits) { sensor = new AnalogInput(port); sensor.setOversampleBits(oversampleBits); sensor.setAverageBits(averageBits); }
public AimShooter() { pot = new AnalogInput(RobotMap.Analog.ShooterPotAngle); pot.setPIDSourceType(PIDSourceType.kDisplacement); motor = new Victor(RobotMap.Pwm.ShooterAngleMotor); anglePID = new PIDSpeedController(pot, motor, "anglePID", "AimShooter"); updatePIDConstants(); anglePID.set(0); }
public MaxSonarUltrasonic(int input) { this.input = new AnalogInput(input); //default values useUnits = true; minVoltage = 0.5; voltageRange = 5.0 - minVoltage; minDistance = 3.0; distanceRange = 60.0 - minDistance; }
public MaxSonarUltrasonic(int input, boolean useUnits, double minVoltage, double maxVoltage, double minDistance, double maxDistance) { this.input = new AnalogInput(input); //only use unit-specific variables if we're using units if(useUnits){ this.useUnits = true; this.minVoltage = minVoltage; voltageRange = maxVoltage - minVoltage; this.minDistance = minDistance; distanceRange = maxDistance - minDistance; } }
public static void init() { // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=CONSTRUCTORS driveTrainLeftMotor = new Talon(0); LiveWindow.addActuator("DriveTrain", "LeftMotor", (Talon) driveTrainLeftMotor); driveTrainRightMotor = new Talon(1); LiveWindow.addActuator("DriveTrain", "RightMotor", (Talon) driveTrainRightMotor); driveTrainRobotDrive = new RobotDrive(driveTrainLeftMotor, driveTrainRightMotor); driveTrainRobotDrive.setSafetyEnabled(true); driveTrainRobotDrive.setExpiration(0.1); driveTrainRobotDrive.setSensitivity(0.5); driveTrainRobotDrive.setMaxOutput(1.0); intakeintakeMotor = new Talon(2); LiveWindow.addActuator("Intake", "intakeMotor", (Talon) intakeintakeMotor); pneumaticSystemCompressor = new Compressor(0); pneumaticSystemDoubleSolenoidPusher = new DoubleSolenoid(0, 0, 1); LiveWindow.addActuator("Pneumatic System", "DoubleSolenoidPusher", pneumaticSystemDoubleSolenoidPusher); sensorBaseUltraSonicMaxbotix = new AnalogInput(0); LiveWindow.addSensor("SensorBase", "UltraSonicMaxbotix", sensorBaseUltraSonicMaxbotix); // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=CONSTRUCTORS }
public void reset() { for (int i = 0; i < motors.length; i++) { if (motors[i] == null) { continue; } if (motors[i] instanceof CANTalon) { ((CANTalon) motors[i]).delete(); } else if (motors[i] instanceof Victor) { ((Victor) motors[i]).free(); } } motors = new SpeedController[64]; for (int i = 0; i < solenoids.length; i++) { if (solenoids[i] == null) { continue; } solenoids[i].free(); } solenoids = new Solenoid[64]; for (int i = 0; i < relays.length; i++) { if (relays[i] == null) { continue; } relays[i].free(); } relays = new Relay[64]; for (int i = 0; i < analogInputs.length; i++) { if (analogInputs[i] == null) { continue; } analogInputs[i].free(); } analogInputs = new AnalogInput[64]; if (compressor != null) compressor.free(); }
public Harvester(SpeedController aHarvesterRollerMotor, SpeedController aHarvesterPivotMotor, IOperatorJoystick aOperatorJoystick, Logger aLogger, AnalogInput aHarvesterPot) { mHarvesterRollerMotor = aHarvesterRollerMotor; mHarvesterPivotMotor = aHarvesterPivotMotor; mOperatorJoystick = aOperatorJoystick; mLogger = aLogger; mHarvesterPot = aHarvesterPot; }
/** * Gyro constructor with a precreated analog channel object. Use this * constructor when the analog channel needs to be shared. * * @param channel The AnalogInput object that the gyro is connected to. Gyros * can only be used on on-board channels 0-1. */ public CustomGyro(AnalogInput channel) { m_analog = channel; if (m_analog == null) { throw new NullPointerException("AnalogInput supplied to Gyro constructor is null"); } initGyro(); calibrate(); }
/** * Gyro constructor with a precreated analog channel object along with * parameters for presetting the center and offset values. Bypasses * calibration. * @param channel The analog channel the gyro is connected to. Gyros can only * be used on on-board channels 0-1. * @param center Preset uncalibrated value to use as the accumulator center value. * @param offset Preset uncalibrated value to use as the gyro offset. */ public CustomGyro(AnalogInput channel, int center, double offset) { m_analog = channel; if (m_analog == null) { throw new NullPointerException("AnalogInput supplied to Gyro constructor is null"); } initGyro(); m_offset = offset; m_center = center; m_analog.setAccumulatorCenter(m_center); m_analog.resetAccumulator(); }
public AnalogIn(int channel, boolean onRIO) { this.onRIO = onRIO; if(onRIO) { input = new AnalogInput(channel); input.setAverageBits(3); } else { Robot.BBBAnalogs[channel] = this; } }
public Sensors() { ahrs = new AHRS(SPI.Port.kMXP); ahrs.reset(); intakeLidar = new AnalogInput(RobotMap.INTAKE_LIDAR_PORT); longLidar = new AnalogInput(RobotMap.LONG_LIDAR_PORT); table = NetworkTable.getTable("SmartDashboard"); }
public Robot() { stick = new Joystick(0); try { /* Construct Digital I/O Objects */ pwm_out_9 = new Victor( getChannelFromPin( PinType.PWM, 9 )); pwm_out_8 = new Jaguar( getChannelFromPin( PinType.PWM, 8 )); dig_in_7 = new DigitalInput( getChannelFromPin( PinType.DigitalIO, 7 )); dig_in_6 = new DigitalInput( getChannelFromPin( PinType.DigitalIO, 6 )); dig_out_5 = new DigitalOutput( getChannelFromPin( PinType.DigitalIO, 5 )); dig_out_4 = new DigitalOutput( getChannelFromPin( PinType.DigitalIO, 4 )); enc_3and2 = new Encoder( getChannelFromPin( PinType.DigitalIO, 3 ), getChannelFromPin( PinType.DigitalIO, 2 )); enc_1and0 = new Encoder( getChannelFromPin( PinType.DigitalIO, 1 ), getChannelFromPin( PinType.DigitalIO, 0 )); /* Construct Analog I/O Objects */ /* NOTE: Due to a board layout issue on the navX MXP board revision */ /* 3.3, there is noticeable crosstalk between AN IN pins 3, 2 and 1. */ /* For that reason, use of pin 3 and pin 2 is NOT RECOMMENDED. */ an_in_1 = new AnalogInput( getChannelFromPin( PinType.AnalogIn, 1 )); an_trig_0 = new AnalogTrigger( getChannelFromPin( PinType.AnalogIn, 0 )); an_trig_0_counter = new Counter( an_trig_0 ); an_out_1 = new AnalogOutput( getChannelFromPin( PinType.AnalogOut, 1 )); an_out_0 = new AnalogOutput( getChannelFromPin( PinType.AnalogOut, 0 )); /* Configure I/O Objects */ pwm_out_9.setSafetyEnabled(false); /* Disable Motor Safety for Demo */ pwm_out_8.setSafetyEnabled(false); /* Disable Motor Safety for Demo */ /* Configure Analog Trigger */ an_trig_0.setAveraged(true); an_trig_0.setLimitsVoltage(MIN_AN_TRIGGER_VOLTAGE, MAX_AN_TRIGGER_VOLTAGE); } catch (RuntimeException ex ) { DriverStation.reportError( "Error instantiating MXP pin on navX MXP: " + ex.getMessage(), true); } }
public DriveTrain() { super(); front_left_motor = new Talon(1); back_left_motor = new Talon(2); front_right_motor = new Talon(3); back_right_motor = new Talon(4); drive = new RobotDrive(front_left_motor, back_left_motor, front_right_motor, back_right_motor); left_encoder = new Encoder(1, 2); right_encoder = new Encoder(3, 4); // Encoders may measure differently in the real world and in // simulation. In this example the robot moves 0.042 barleycorns // per tick in the real world, but the simulated encoders // simulate 360 tick encoders. This if statement allows for the // real robot to handle this difference in devices. if (Robot.isReal()) { left_encoder.setDistancePerPulse(0.042); right_encoder.setDistancePerPulse(0.042); } else { // Circumference in ft = 4in/12(in/ft)*PI left_encoder.setDistancePerPulse((4.0/12.0*Math.PI) / 360.0); right_encoder.setDistancePerPulse((4.0/12.0*Math.PI) / 360.0); } rangefinder = new AnalogInput(6); gyro = new AnalogGyro(1); // Let's show everything on the LiveWindow LiveWindow.addActuator("Drive Train", "Front_Left Motor", (Talon) front_left_motor); LiveWindow.addActuator("Drive Train", "Back Left Motor", (Talon) back_left_motor); LiveWindow.addActuator("Drive Train", "Front Right Motor", (Talon) front_right_motor); LiveWindow.addActuator("Drive Train", "Back Right Motor", (Talon) back_right_motor); LiveWindow.addSensor("Drive Train", "Left Encoder", left_encoder); LiveWindow.addSensor("Drive Train", "Right Encoder", right_encoder); LiveWindow.addSensor("Drive Train", "Rangefinder", rangefinder); LiveWindow.addSensor("Drive Train", "Gyro", gyro); }
/** * Return the actual angle in radians that the robot is currently facing. * * The angle is based on the current accumulator value corrected by the * oversampling rate, the gyro type and the A/D calibration values. The * angle is continuous, that is it will continue from past 2pi radians. This * allows algorithms that wouldn't want to see a discontinuity in the gyro * output as it sweeps past from 2pi to 0 on the second time around. * * @return the current heading of the robot in radians. This heading is * based on integration of the returned rate from the gyro. */ @Override public double getAngle() { analogInput.getAccumulatorOutput(result); long value = result.value - (long) (result.count * offset); double scaledValue = value * 1e-9 * analogInput.getLSBWeight() * (1 << analogInput.getAverageBits()) / (AnalogInput.getGlobalSampleRate() * voltsPerRadianPerSecond); return scaledValue + angleOffset; }
/** * Gyro constructor with a pre-created analog channel object. Use this * constructor when the analog channel needs to be shared. * * @param channel * The AnalogInput object that the gyro is connected to. Gyros * can only be used on on-board channels 0-1. */ public HVAGyro(AnalogInput channel) { m_analog = channel; if (m_analog == null) { throw new NullPointerException("AnalogInput supplied to Gyro constructor is null"); } initGyro(); }
public InfraredSwitch(int pInfraredDetectorChannel, AnalogInput pAnalogVoltageMeter, double pHammerLevel, int pOnState, boolean pForceStateChange) { infraredDetector = new AnalogInput(pInfraredDetectorChannel); analogVoltageMeter = pAnalogVoltageMeter; hammerLevel = pHammerLevel; onState = pOnState; setForceStateChange(pForceStateChange); }
public Potentiometer(int pPotentiometerChannel, AnalogInput pAnalogVoltageMeter, boolean pReverse) { potentiometer = new AnalogInput(pPotentiometerChannel); analogVoltageMeter = pAnalogVoltageMeter; scaled = false; scale = 0.0; minValue = 0.0; maxValue = 0.0; reverse = pReverse; }
public Potentiometer(int pPotentiometerChannel, AnalogInput pAnalogVoltageMeter, boolean pReverse, double pScale, double pMinValue, double pMaxValue) { potentiometer = new AnalogInput(pPotentiometerChannel); analogVoltageMeter = pAnalogVoltageMeter; scaled = true; scale = pScale; minValue = pMinValue; maxValue = pMaxValue; reverse = pReverse; }
private double getDistance(AnalogInput ultrasonic) { //return ultrasonic.getValue() * kValueToInches; return ultrasonic.getAverageVoltage()*kVoltageToInches; }
private double getDistance(AnalogInput ultrasonic) { return ultrasonic.getValue() * kValueToInches; }
public AnalogUltrasonic(int pin){ input = new AnalogInput(pin); }
public UltraSonic() { Robot.logList.add(this); ultraSonic = new AnalogInput(RobotMap.Ports.ultraSonic); }
public IntakeRoller() { motor = new Victor(RobotMap.Pwm.RollerVictor); intakeSensor = new AnalogInput(RobotMap.Analog.IntakeSensor); }
public void robotInit() { System.out.println("Pre RobotMap Init"); RobotMap.init(); System.out.println("Post RobotMap Init"); System.out.println("Elevator"); shooterAim = new ShooterAim(); System.out.println("Shooter Aim"); shooterWheels = new ShooterWheels(); System.out.println("Shooter Wheels"); nav6 = new Nav6(); if(nav6 != null && nav6.isValid()) { System.out.println("Nav6"); } else { nav6 = null; System.out.println("Nav6 is offline. PID disabled."); } pneumatics = new Pneumatics(); System.out.println("Pneumatics"); driveTrain = new DriveTrain(); System.out.println("DriveTrain"); oi = new OI(); System.out.println("OI"); ultrasonicSensor = new AnalogInput(3); System.out.println("Ultrasonic"); table = NetworkTable.getTable("GRIP"); System.out.println("Network"); accelerometerSampling = new AccelerometerSampling(); System.out.println("Accelerometer"); dataDisplayer = new DataDisplayer(); System.out.println("DataDisplayer"); System.out.println("Robot init done"); autonomousCommand = new ObstacleHighGoalAndReset(); System.out.println("Autonomous command initialized"); (new SafeArmsUp()).start(); System.out.println("Arms retracted"); }
public DistanceSensorSubsystem() { distanceSensor = new AnalogInput(RobotMap.Electrical.DISTANCE_SENSOR); }
public PressureSensorSubsystem() { pressureSensor = new AnalogInput(RobotMap.Electrical.PRESSURE_SENSOR); }
public MaxbotixMB1013(int AINPort){ port=AINPort; ain=new AnalogInput(port); }
public Drive() { // SPEED CONTROLLER PORTS frontLeftTalon = new Talon(RobotMap.Pwm.FrontLeftDrive); rearLeftTalon = new Talon(RobotMap.Pwm.RearLeftDrive); frontRightTalon = new Talon(RobotMap.Pwm.FrontRightDrive); rearRightTalon = new Talon(RobotMap.Pwm.RearRightDrive); // ULTRASONIC SENSORS leftAngleIR = new AnalogInput(RobotMap.Analog.LeftAngleIR); rightAngleIR = new AnalogInput(RobotMap.Analog.RightAngleIR); leftCenterIR = new AnalogInput(RobotMap.Analog.LeftCenterIR); rightCenterIR = new AnalogInput(RobotMap.Analog.RightCenterIR); // YAWRATE SENSOR gyro = new AnalogGyro(RobotMap.Analog.Gryo); // ENCODER PORTS frontLeftEncoder = new Encoder(RobotMap.Encoders.FrontLeftDriveA, RobotMap.Encoders.FrontLeftDriveB); rearLeftEncoder = new Encoder(RobotMap.Encoders.RearLeftDriveA, RobotMap.Encoders.RearLeftDriveB); frontRightEncoder = new Encoder(RobotMap.Encoders.FrontRightDriveA, RobotMap.Encoders.FrontRightDriveB); rearRightEncoder = new Encoder(RobotMap.Encoders.RearRightDriveA, RobotMap.Encoders.RearRightDriveB); // ENCODER MATH frontLeftEncoder.setDistancePerPulse(DistancePerPulse); frontLeftEncoder.setPIDSourceType(PIDSourceType.kRate); frontRightEncoder.setDistancePerPulse(DistancePerPulse); frontRightEncoder.setPIDSourceType(PIDSourceType.kRate); rearLeftEncoder.setDistancePerPulse(DistancePerPulse); rearLeftEncoder.setPIDSourceType(PIDSourceType.kRate); rearRightEncoder.setDistancePerPulse(DistancePerPulse); rearRightEncoder.setPIDSourceType(PIDSourceType.kRate); // PID SPEED CONTROLLERS frontLeftPID = new PIDSpeedController(frontLeftEncoder, frontLeftTalon, "Drive", "Front Left"); frontRightPID = new PIDSpeedController(frontRightEncoder, frontRightTalon, "Drive", "Front Right"); rearLeftPID = new PIDSpeedController(rearLeftEncoder, rearLeftTalon, "Drive", "Rear Left"); rearRightPID = new PIDSpeedController(rearRightEncoder, rearRightTalon, "Drive", "Rear Right"); // DRIVE DECLARATION robotDrive = new RobotDrive(frontLeftPID, rearLeftPID, frontRightPID, rearRightPID); robotDrive.setExpiration(0.1); // MOTOR INVERSIONS robotDrive.setInvertedMotor(MotorType.kFrontRight, true); robotDrive.setInvertedMotor(MotorType.kRearRight, true); LiveWindow.addActuator("Drive", "Front Left Talon", frontLeftTalon); LiveWindow.addActuator("Drive", "Front Right Talon", frontRightTalon); LiveWindow.addActuator("Drive", "Rear Left Talon", rearLeftTalon); LiveWindow.addActuator("Drive", "Rear Right Talon", rearRightTalon); LiveWindow.addSensor("Drive Encoders", "Front Left Encoder", frontLeftEncoder); LiveWindow.addSensor("Drive Encoders", "Front Right Encoder", frontRightEncoder); LiveWindow.addSensor("Drive Encoders", "Rear Left Encoder", rearLeftEncoder); LiveWindow.addSensor("Drive Encoders", "Rear Right Encoder", rearRightEncoder); }
/** * This function is run when the robot is first started up and should be * used for any initialization code. */ public void robotInit() { RobotMap.init(); //maxTankDriveSpeed = prefs.getDouble("MaxTankDriveSpeed", 1.0); // These need to be added to the smartdashboard or the program will crash // see this link for instructions http://wpilib.screenstepslive.com/s/3120/m/7932/l/81114-setting-robot-preferences-from-smartdashboard //try //{ //MaxElevatorSpeed = prefs.getDouble("MaxElevatorSpeed", MaxElevatorSpeed); //RampTimeInSeconds = prefs.getDouble("RamptTimeInSeconds", RampTimeInSeconds); //CushyStopSteps = prefs.getInt("CushyStopSteps", CushyStopSteps); //StopRampFactor = prefs.getDouble("StopRampFactor", StopRampFactor); //} //catch(Exception ex) //{ // System.out.println(ex.getMessage()); //} //System.out.println("Ramp Time: " + String.format("%.3f", RampTimeInSeconds)); // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=CONSTRUCTORS driveTrain = new DriveTrain(); frontElevator = new FrontElevator(); backElevator = new BackElevator(); binArm = new BinArm(); autonSwitch = new AutonSwitch(); // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=CONSTRUCTORS // OI must be constructed after subsystems. If the OI creates Commands //(which it very likely will), subsystems are not guaranteed to be // constructed yet. Thus, their requires() statements may grab null // pointers. Bad news. Don't move it. oi = new OI(); // instantiate the command used for the autonomous period // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=AUTONOMOUS autonomousCommand = new AutonomousCommandGroup(); // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=AUTONOMOUS AnalogInput.setGlobalSampleRate(1000); }
