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

diff --git a/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java b/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java
index 0d1db15b930268ccea2a86b65627118080932e14..be0103cf2c81b99add42e14ec12fda7504865854 100644 (file)
--- a/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java
+++ b/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java
@@ -15,19 +15,23 @@ 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;
+  // Encoder PID Proportional Constants P, I, and D
+  private static double EP = 0.013, EI = 0.000015, ED = -0.002;
+
+  // Gyro PID Constants P, I, and D
+  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 Encoder leftEncoder, rightEncoder;
-
-  public static Lidar leftLidar;
-  public static Lidar rightLidar;
-
   private CANTalon frontLeft, frontRight, rearLeft, rearRight;
   private RobotDrive robotDrive;
 
@@ -57,10 +61,6 @@ public class DriveTrain extends PIDSubsystem {
     rearRight = new CANTalon(Constants.DriveTrain.REAR_RIGHT);
 
     robotDrive = new RobotDrive(frontLeft, rearLeft, frontRight, rearRight);
-
-    leftLidar = new Lidar(I2C.Port.kOnboard);
-    rightLidar = new Lidar(I2C.Port.kOnboard); // TODO: find port for second
-                                               // 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,
@@ -79,7 +79,7 @@ public class DriveTrain extends PIDSubsystem {
     gyro.start();
 
     leftGearPiston = new DoubleSolenoid(Constants.DriveTrain.LEFT_FORWARD,
-        +Constants.DriveTrain.LEFT_REVERSE);
+        Constants.DriveTrain.LEFT_REVERSE);
     rightGearPiston = new DoubleSolenoid(Constants.DriveTrain.RIGHT_FORWARD,
         Constants.DriveTrain.RIGHT_REVERSE);
   }
@@ -89,6 +89,7 @@ public class DriveTrain extends PIDSubsystem {
     setDefaultCommand(new JoystickDrive());
   }
 
+  // Print tne PID Output
   public void printOutput() {
     System.out.println("PIDOutput: " + pidOutput);
   }
@@ -97,6 +98,8 @@ public class DriveTrain extends PIDSubsystem {
     return (leftEncoder.getDistance() + rightEncoder.getDistance()) / 2;
   }
 
+  // Whether or not the PID Controller thinks we have reached the target
+  // setpoint
   public boolean reachedTarget() {
     if (this.onTarget()) {
       this.disable();
@@ -115,14 +118,6 @@ public class DriveTrain extends PIDSubsystem {
     rightEncoder.reset();
   }
 
-  public double getLeftLidarDistance() {
-    return leftLidar.pidGet();
-  }
-
-  public double getRightLidarDistance() {
-    return rightLidar.pidGet();
-  }
-
   public double getRightSpeed() {
     return rightEncoder.getRate(); // in inches per second
   }
@@ -143,6 +138,8 @@ public class DriveTrain extends PIDSubsystem {
     return leftEncoder.getDistance(); // in inches
   }
 
+  // Get error between the setpoint of PID Controller and the current state of
+  // the robot
   public double getError() {
     if (DRIVE_MODE == Constants.DriveTrain.ENCODER_MODE)
       return Math.abs(this.getSetpoint() - getAvgEncoderDistance());
@@ -170,10 +167,14 @@ public class DriveTrain extends PIDSubsystem {
     System.out.println("Gyro Angle" + -this.getGyroAngle());
   }
 
+  /*
+   * returns the PID output that is returned by the PID Controller
+   */
   public double getOutput() {
     return pidOutput;
   }
 
+  // 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);
@@ -201,6 +202,15 @@ public class DriveTrain extends PIDSubsystem {
     return DRIVE_MODE;
   }
 
+  /*
+   * Method is a required method that the PID Subsystem uses to return the
+   * calculated PID value to the driver
+   * 
+   * @param Gives the user the output from the PID algorithm that is calculated
+   * internally
+   * 
+   * Body: Uses the output, does some filtering and drives the robot
+   */
   @Override
   protected void usePIDOutput(double output) {
     double left = 0;
@@ -211,7 +221,8 @@ public class DriveTrain extends PIDSubsystem {
         output = Math.signum(output) * 0.3;
       left = output;
       right = output + drift * kp / 10;
-    } else if (DRIVE_MODE == Constants.DriveTrain.GYRO_MODE) {
+    }
+    else if (DRIVE_MODE == Constants.DriveTrain.GYRO_MODE) {
       left = output;
       right = -output;
     }
@@ -224,6 +235,13 @@ public class DriveTrain extends PIDSubsystem {
     return sensorFeedback();
   }
 
+  /*
+   * Checks the drive mode
+   * 
+   * @return the current state of the robot in each state
+   * Average distance from both sides of tank drive for Encoder Mode
+   * Angle from the gyro in GYRO_MODE
+   */
   private double sensorFeedback() {
     if (DRIVE_MODE == Constants.DriveTrain.ENCODER_MODE)
       return getAvgEncoderDistance();
@@ -234,17 +252,37 @@ public class DriveTrain extends PIDSubsystem {
       return 0;
   }
 
+  /*
+   * @param left and right setpoints to set to the left and right side of tank
+   * inverted is for Logan, wants the robot to invert all controls left = right
+   * and right = left
+   * negative input is required for the regular rotation because RobotDrive
+   * tankdrive method drives inverted
+   */
   public void drive(double left, double right) {
     robotDrive.tankDrive(-left, -right);
     // dunno why but inverted drive (- values is forward)
   }
 
+  /*
+   * 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();
@@ -254,6 +292,11 @@ public class DriveTrain extends PIDSubsystem {
     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();
@@ -265,6 +308,14 @@ public class DriveTrain extends PIDSubsystem {
     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();
@@ -280,22 +331,39 @@ public class DriveTrain extends PIDSubsystem {
     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);