- // dunno why but inverted drive (- values is forward)
- if (!Constants.DriveTrain.inverted)
- robotDrive.tankDrive(-left, -right);
- else
- robotDrive.tankDrive(right, left);
- }
-
- /*
- * 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);