Add comments to all of the driveTrain PID commands and methods to explain what is...

[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 c88d4c42f9e1c92d00e4b3a17480bc20ca7f6939..be0103cf2c81b99add42e14ec12fda7504865854 100644 (file)
--- a/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java
+++ b/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java
@@ -37,9 +37,23 @@ 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(EP, EI, ED);
+    super(kp, ki, kd);
 
     frontLeft = new CANTalon(Constants.DriveTrain.FRONT_LEFT);
     frontRight = new CANTalon(Constants.DriveTrain.FRONT_RIGHT);
@@ -68,7 +82,6 @@ public class DriveTrain extends PIDSubsystem {
         Constants.DriveTrain.LEFT_REVERSE);
     rightGearPiston = new DoubleSolenoid(Constants.DriveTrain.RIGHT_FORWARD,
         Constants.DriveTrain.RIGHT_REVERSE);
-    Constants.DriveTrain.inverted = false;
   }
 
   @Override
@@ -164,9 +177,9 @@ 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(EP, EI, ED);
+      this.getPIDController().setPID(kp, ki, kd);
     else
-      this.getPIDController().setPID(GP, GD, GI);
+      this.getPIDController().setPID(gp, gd, gi);
   }
 
   public CANTalon getFrontLeft() {
@@ -207,7 +220,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 * EP / 10;
+      right = output + drift * kp / 10;
     }
     else if (DRIVE_MODE == Constants.DriveTrain.GYRO_MODE) {
       left = output;
@@ -247,12 +260,8 @@ public class DriveTrain extends PIDSubsystem {
    * tankdrive method drives inverted
    */
   public void drive(double left, double right) {
+    robotDrive.tankDrive(-left, -right);
     // dunno why but inverted drive (- values is forward)
-    if (!Constants.DriveTrain.inverted)
-      robotDrive.tankDrive(-left,
-          -right);
-    else
-      robotDrive.tankDrive(right, left);
   }
 
   /*
@@ -291,7 +300,7 @@ 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(gp, gi, gd);
 
     this.setAbsoluteTolerance(gyroTolerance);
     this.setOutputRange(-1.0, 1.0);