Remove sendable choosers, fix joystick driving bug, use right twist instead of right...

[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..eb819f1cb37aa6ed900d2a18cc046bc0c8dcde73 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,17 @@ 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);
+    // Handle flipping of the "front" of the robot
+    double k = (isFlipped() ? -1 : 1);
+
+    // During teleop, leftY is throttle, rightX is twist.
+    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);
   }
 
   /**