Put drivetrain constants in Constants.java

[3501/stronghold-2016] / src / org / usfirst / frc / team3501 / robot / subsystems / DriveTrain.java

diff --git a/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java b/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java
index 2b11491abf728705e52b4f63381ee5fc509acbf0..a4bba48b5fe7bd276b372f0144995d81493d7daf 100644 (file)
--- a/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java
+++ b/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java
@@ -16,18 +16,9 @@ import edu.wpi.first.wpilibj.RobotDrive;
 import edu.wpi.first.wpilibj.command.PIDSubsystem;
 
 public class DriveTrain extends PIDSubsystem {
-  private static double kp = 0.013, ki = 0.000015, kd = -0.002;
-  private static double gp = 0.018, gi = 0.000015, gd = 0;
-  private static double pidOutput = 0;
-
-  // PID Controller tolerances for the error
-  private static double encoderTolerance = 8.0, gyroTolerance = 5.0;
-
   // Current Drive Mode Default Drive Mode is Manual
   private int DRIVE_MODE = 1;
-
-  // Different Drive Modes
-  private static final int MANUAL_MODE = 1, ENCODER_MODE = 2, GYRO_MODE = 3;
+  private static double pidOutput = 0;
 
   private Encoder leftEncoder, rightEncoder;
 
@@ -39,23 +30,13 @@ public class DriveTrain extends PIDSubsystem {
 
   private GyroLib gyro;
   private DoubleSolenoid leftGearPiston, rightGearPiston;
+
   // Drivetrain specific constants that relate to the inches per pulse value for
   // the encoders
-  private final static double WHEEL_DIAMETER = 6.0; // in inches
-  private final static double PULSES_PER_ROTATION = 256; // in pulses
-  private final static double OUTPUT_SPROCKET_DIAMETER = 2.0; // in inches
-  private final static double WHEEL_SPROCKET_DIAMETER = 3.5; // in inches
-  public final static double INCHES_PER_PULSE = (((Math.PI)
-      * OUTPUT_SPROCKET_DIAMETER / PULSES_PER_ROTATION)
-      / WHEEL_SPROCKET_DIAMETER) * WHEEL_DIAMETER;
-
-  // Drivetrain specific constants that relate to the PID controllers
-  private final static double Kp = 1.0, Ki = 0.0,
-      Kd = 0.0 * (OUTPUT_SPROCKET_DIAMETER / PULSES_PER_ROTATION)
-          / (WHEEL_SPROCKET_DIAMETER) * WHEEL_DIAMETER;
 
   public DriveTrain() {
-    super(kp, ki, kd);
+    super(Constants.DriveTrain.kp, Constants.DriveTrain.ki,
+        Constants.DriveTrain.kd);
 
     frontLeft = new CANTalon(Constants.DriveTrain.FRONT_LEFT);
     frontRight = new CANTalon(Constants.DriveTrain.FRONT_RIGHT);
@@ -66,7 +47,7 @@ public class DriveTrain extends PIDSubsystem {
 
     leftLidar = new Lidar(I2C.Port.kOnboard);
     rightLidar = new Lidar(I2C.Port.kOnboard); // TODO: find port for second
-                                               // lidar
+    // lidar
     leftEncoder = new Encoder(Constants.DriveTrain.ENCODER_LEFT_A,
         Constants.DriveTrain.ENCODER_LEFT_B, false, EncodingType.k4X);
     rightEncoder = new Encoder(Constants.DriveTrain.ENCODER_RIGHT_A,
@@ -192,9 +173,11 @@ public class DriveTrain extends PIDSubsystem {
   // Updates the PID constants based on which control mode is being used
   public void updatePID() {
     if (DRIVE_MODE == Constants.DriveTrain.ENCODER_MODE)
-      this.getPIDController().setPID(kp, ki, kd);
+      this.getPIDController().setPID(Constants.DriveTrain.kp,
+          Constants.DriveTrain.ki, Constants.DriveTrain.kd);
     else
-      this.getPIDController().setPID(gp, gd, gi);
+      this.getPIDController().setPID(Constants.DriveTrain.gp,
+          Constants.DriveTrain.gd, Constants.DriveTrain.gi);
   }
 
   public CANTalon getFrontLeft() {
@@ -235,7 +218,7 @@ public class DriveTrain extends PIDSubsystem {
       if (Math.abs(output) > 0 && Math.abs(output) < 0.3)
         output = Math.signum(output) * 0.3;
       left = output;
-      right = output + drift * kp / 10;
+      right = output + drift * Constants.DriveTrain.kp / 10;
     } else if (DRIVE_MODE == Constants.DriveTrain.GYRO_MODE) {
       left = output;
       right = -output;
@@ -305,7 +288,7 @@ public class DriveTrain extends PIDSubsystem {
   public void setEncoderPID() {
     DRIVE_MODE = Constants.DriveTrain.ENCODER_MODE;
     this.updatePID();
-    this.setAbsoluteTolerance(encoderTolerance);
+    this.setAbsoluteTolerance(Constants.DriveTrain.encoderTolerance);
     this.setOutputRange(-1.0, 1.0);
     this.setInputRange(-200.0, 200.0);
     this.enable();
@@ -319,9 +302,10 @@ public class DriveTrain extends PIDSubsystem {
   private void setGyroPID() {
     DRIVE_MODE = Constants.DriveTrain.GYRO_MODE;
     this.updatePID();
-    this.getPIDController().setPID(gp, gi, gd);
+    this.getPIDController().setPID(Constants.DriveTrain.gp,
+        Constants.DriveTrain.gi, Constants.DriveTrain.gd);
 
-    this.setAbsoluteTolerance(gyroTolerance);
+    this.setAbsoluteTolerance(Constants.DriveTrain.gyroTolerance);
     this.setOutputRange(-1.0, 1.0);
     this.setInputRange(-360.0, 360.0);
     this.enable();