+
+ public void arcadeDrive(double y, double twist) {
+ robotDrive.arcadeDrive(y, twist);
+ }
+
+ /*
+ * constrains the distance to within -100 and 100 since we aren't going to
+ * drive more than 100 inches
+ *
+ * Configure Encoder PID
+ *
+ * Sets the setpoint to the PID subsystem
+ */
+ public void driveDistance(double dist, double maxTimeOut) {
+ dist = MathLib.constrain(dist, -100, 100);
+ setEncoderPID();
+ setSetpoint(dist);
+ }
+
+ /*
+ * Sets the encoder mode Updates the PID constants sets the tolerance and sets
+ * output/input ranges Enables the PID controllers
+ */
+ public void setEncoderPID() {
+ DRIVE_MODE = Constants.DriveTrain.ENCODER_MODE;
+ this.updatePID();
+ this.setAbsoluteTolerance(Constants.DriveTrain.encoderTolerance);
+ this.setOutputRange(-1.0, 1.0);
+ this.setInputRange(-200.0, 200.0);
+ this.enable();
+ }
+
+ /*
+ * Sets the Gyro Mode Updates the PID constants, sets the tolerance and sets
+ * output/input ranges Enables the PID controllers
+ */
+ private void setGyroPID() {
+ DRIVE_MODE = Constants.DriveTrain.GYRO_MODE;
+ this.updatePID();
+ this.getPIDController().setPID(Constants.DriveTrain.gp,
+ Constants.DriveTrain.gi, Constants.DriveTrain.gd);
+
+ this.setAbsoluteTolerance(Constants.DriveTrain.gyroTolerance);
+ this.setOutputRange(-1.0, 1.0);
+ this.setInputRange(-360.0, 360.0);
+ this.enable();
+ }
+
+ /*
+ * Turning method that should be used repeatedly in a command
+ *
+ * First constrains the angle to within -360 and 360 since that is as much as
+ * we need to turn
+ *
+ * Configures Gyro PID and sets the setpoint as an angle
+ */
+ public void turnAngle(double angle) {
+ angle = MathLib.constrain(angle, -360, 360);
+ setGyroPID();
+ setSetpoint(angle);
+ }
+
+ public void setMotorSpeeds(double left, double right) {
+ // positive setpoint to left side makes it go backwards
+ // positive setpoint to right side makes it go forwards.
+ frontLeft.set(-left);
+ rearLeft.set(-left);
+ frontRight.set(right);
+ rearRight.set(right);
+ }
+
+ /*
+ * @return a value that is the current setpoint for the piston kReverse or
+ * kForward
+ */
+ public Value getLeftGearPistonValue() {
+ return leftGearPiston.get();
+ }
+
+ /*
+ * @return a value that is the current setpoint for the piston kReverse or
+ * kForward
+ */
+ public Value getRightGearPistonValue() {
+ return rightGearPiston.get();
+ }
+
+ /*
+ * Changes the ball shift gear assembly to high
+ */
+ public void setHighGear() {
+ changeGear(Constants.DriveTrain.HIGH_GEAR);
+ }
+
+ /*
+ * Changes the ball shift gear assembly to low
+ */
+ public void setLowGear() {
+ changeGear(Constants.DriveTrain.LOW_GEAR);
+ }
+
+ /*
+ * changes the gear to a DoubleSolenoid.Value
+ */
+ public void changeGear(DoubleSolenoid.Value gear) {
+ leftGearPiston.set(gear);
+ rightGearPiston.set(gear);
+ }
+