public void calcEncoders() {
leftFrontDriveEncoder.calc();
rightFrontDriveEncoder.calc();
leftRearDriveCounter.calc();
rightRearDriveCounter.calc();
catapultEncoder.calc();
frontIntakeTiltPotentiometer.run();
rearIntakeTiltPotentiometer.run();
}
public void resetEncoders() {
leftFrontDriveEncoder.reset();
rightFrontDriveEncoder.reset();
leftRearDriveCounter.reset();
rightRearDriveCounter.reset();
catapultEncoder.reset();
frontIntakeTiltPotentiometer.reset();
rearIntakeTiltPotentiometer.reset();
gyro.reset();
}
public void loadParameters() {
double frontIntakePotentiometerLow = params.getAsDouble("P_FrontIntakeVoltageLow", 4.5);
double frontIntakePotentiometerHigh = params.getAsDouble("P_FrontIntakeVoltageHigh", 0.5);
double rearIntakePotentiometerLow = params.getAsDouble("P_RearIntakeVoltageLow", 4.5);
double rearIntakePotentiometerHigh = params.getAsDouble("P_RearIntakeVoltageHigh", 0.5);
frontIntakeTiltPotentiometer.setRange(
frontIntakePotentiometerLow, frontIntakePotentiometerHigh);
rearIntakeTiltPotentiometer.setRange(rearIntakePotentiometerLow, rearIntakePotentiometerHigh);
}
private void startEncoders() {
// 1 foot = ??? clicks
leftFrontDriveEncoder.start();
rightFrontDriveEncoder.start();
leftRearDriveCounter.start();
rightRearDriveCounter.start();
catapultEncoder.start();
frontIntakeTiltPotentiometer.reset();
rearIntakeTiltPotentiometer.reset();
gyro.reset();
}
public SensorInput() {
watchdog = Watchdog.getInstance();
params = Parameters.getTeleopInstance();
// ----- Encoders/Counters -----
leftFrontDriveEncoder =
new TorqueEncoder(
Ports.LEFT_FRONT_DRIVE_ENCODER_SIDECAR,
Ports.LEFT_FRONT_DRIVE_ENCODER_A_PORT,
Ports.LEFT_FRONT_DRIVE_ENCODER_SIDECAR,
Ports.LEFT_FRONT_DRIVE_ENCODER_B_PORT,
false);
leftRearDriveCounter =
new TorqueCounter(
Ports.LEFT_REAR_DRIVE_ENCODER_SIDECAR, Ports.LEFT_REAR_DRIVE_ENCODER_PORT);
rightFrontDriveEncoder =
new TorqueEncoder(
Ports.RIGHT_FRONT_DRIVE_ENCODER_SIDECAR,
Ports.RIGHT_FRONT_DRIVE_ENCODER_A_PORT,
Ports.RIGHT_FRONT_DRIVE_ENCODER_SIDECAR,
Ports.RIGHT_FRONT_DRIVE_ENCODER_B_PORT,
false);
rightRearDriveCounter =
new TorqueCounter(
Ports.RIGHT_REAR_DRIVE_ENCODER_SIDECAR, Ports.RIGHT_REAR_DRIVE_ENCODER_PORT);
catapultEncoder =
new TorqueEncoder(
Ports.CATAPULT_ENCODER_SIDECAR,
Ports.CATAPULT_ENCODER_A_PORT,
Ports.CATAPULT_ENCODER_SIDECAR,
Ports.CATAPULT_ENCODER_B_PORT,
false);
// ----- Gyro -----
gyroChannel = new AnalogChannel(Ports.GYRO_PORT);
gyro = new Gyro(gyroChannel);
gyro.reset();
gyro.setSensitivity(Constants.GYRO_SENSITIVITY);
// ----- Potentiometers -----
rearIntakeTiltPotentiometer = new TorquePotentiometer(Ports.REAR_INTAKE_TILT_POT_PORT);
frontIntakeTiltPotentiometer = new TorquePotentiometer(Ports.FRONT_INTAKE_TILT_POT_PORT);
frontIntakeTiltPotentiometer.setRange(
Constants.FRONT_INTAKE_POTENTIOMETER_LOW, Constants.FRONT_INTAKE_POTENTIOMETER_HIGH);
rearIntakeTiltPotentiometer.setRange(
Constants.REAR_INTAKE_POTENTIOMETER_LOW, Constants.REAR_INTAKE_POTENTIOMETER_HIGH);
// ----- Buttons -----
catapultLimitSwitch =
new DigitalInput(Ports.CATAPULT_LIMIT_SWITCH_B_SIDECAR, Ports.CATAPULT_LIMIT_SWITCH_B_PORT);
catapultLimitSwitchB =
new DigitalInput(Ports.CATAPULT_LIMIT_SWITCH_SIDECAR, Ports.CATAPULT_LIMIT_SWITCH_PORT);
frontIntakeButton = new DigitalInput(Ports.INTAKE_BUTTON_SIDECAR, Ports.FRONT_INTAKE_BUTTON);
rearIntakeButton = new DigitalInput(Ports.INTAKE_BUTTON_SIDECAR, Ports.REAR_INTAKE_BUTTON);
// pressureSensor = new AnalogChannel(Ports.ANALOG_PRESSURE_PORT);
startEncoders();
}
public double getFrontIntakeTiltVoltage() {
return frontIntakeTiltPotentiometer.getRaw();
}
public double getRearIntakeTiltVoltage() {
return rearIntakeTiltPotentiometer.getRaw();
}
public double getRearIntakeTiltPotentiometer() {
return rearIntakeTiltPotentiometer.get();
}
public double getFrontIntakeTiltPotentiometer() {
return frontIntakeTiltPotentiometer.get();
}
