Finally got this code reviewed, bugfixes

[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 c68b472e2ff3a5878dc32569aa702ed4aafd99db..73dcde7087954b041c3e102516b973dfa4de42d1 100644 (file)
--- a/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java
+++ b/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java
@@ -12,7 +12,9 @@ import edu.wpi.first.wpilibj.RobotDrive;
 import edu.wpi.first.wpilibj.command.PIDSubsystem;
 
 public class DriveTrain extends PIDSubsystem {
+  // Determines if the "front" of the robot has been reversed
   private boolean outputFlipped = false;
+
   private static double pidOutput = 0;
 
   private Encoder leftEncoder, rightEncoder;
@@ -49,9 +51,12 @@ public class DriveTrain extends PIDSubsystem {
     this.disable();
 
     leftGearPiston = new DoubleSolenoid(Constants.DriveTrain.LEFT_SHIFT_MODULE,
-        Constants.DriveTrain.LEFT_SHIFT_FORWARD, Constants.DriveTrain.LEFT_SHIFT_REVERSE);
-    rightGearPiston = new DoubleSolenoid(Constants.DriveTrain.RIGHT_SHIFT_MODULE,
-        Constants.DriveTrain.RIGHT_SHIFT_FORWARD, Constants.DriveTrain.RIGHT_SHIFT_REVERSE);
+        Constants.DriveTrain.LEFT_SHIFT_FORWARD,
+        Constants.DriveTrain.LEFT_SHIFT_REVERSE);
+    rightGearPiston = new DoubleSolenoid(
+        Constants.DriveTrain.RIGHT_SHIFT_MODULE,
+        Constants.DriveTrain.RIGHT_SHIFT_FORWARD,
+        Constants.DriveTrain.RIGHT_SHIFT_REVERSE);
   }
 
   @Override
@@ -80,7 +85,7 @@ public class DriveTrain extends PIDSubsystem {
   }
 
   public void stop() {
-    drive(0, 0);
+    setMotorSpeeds(0, 0);
   }
 
   public void resetEncoders() {
@@ -169,7 +174,7 @@ public class DriveTrain extends PIDSubsystem {
       output = Math.signum(output) * 0.3;
     left = output;
     right = output + drift * Constants.DriveTrain.kp / 10;
-    drive(left, right);
+    setMotorSpeeds(left, right);
     pidOutput = output;
   }
 
@@ -188,23 +193,33 @@ public class DriveTrain extends PIDSubsystem {
     return getAvgEncoderDistance();
   }
 
-  /*
-   * @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);
+  public void joystickDrive(double left, double right) {
+    int type = Constants.DriveTrain.DRIVE_TYPE;
+
+    // Handle flipping of the "front" of the robot
+    double k = (isFlipped() ? -1 : 1);
+
+    if (type == Constants.DriveTrain.TANK) {
+      // TODO Test this for negation and for voltage vs. [-1,1] outputs
+      double leftSpeed = (-frontLeft.get() + -rearLeft.get()) / 2;
+      double rightSpeed = (-frontRight.get() + -rearRight.get()) / 2;
+      left = ((Constants.DriveTrain.kADJUST - 1) * leftSpeed + left)
+          / Constants.DriveTrain.kADJUST;
+      right = ((Constants.DriveTrain.kADJUST - 1) * rightSpeed + right)
+          / Constants.DriveTrain.kADJUST;
+      robotDrive.tankDrive(-left * k, -right * k);
+    } else if (type == Constants.DriveTrain.ARCADE) {
+      double speed = (-frontLeft.get() + -rearLeft.get() + -frontRight.get() + -rearRight
+          .get()) / 2;
+      left = ((Constants.DriveTrain.kADJUST - 1) * speed + left)
+          / Constants.DriveTrain.kADJUST;
+      robotDrive.arcadeDrive(left * k, right);
+    }
   }
 
   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);
+    double k = (isFlipped() ? -1 : 1);
+    robotDrive.tankDrive(-left * k, -right * k);
   }
 
   /**