Modify code to allow switching of joystick drive type (tank vs. arcade)

[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..779bd9dec0ad0e0c98ad0fe440d6ffc181babe78 100644 (file)
--- a/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java
+++ b/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java
@@ -1,6 +1,7 @@
 package org.usfirst.frc.team3501.robot.subsystems;
 
 import org.usfirst.frc.team3501.robot.Constants;
+import org.usfirst.frc.team3501.robot.Robot;
 import org.usfirst.frc.team3501.robot.commands.driving.JoystickDrive;
 
 import edu.wpi.first.wpilibj.CANTalon;
@@ -49,9 +50,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 +84,7 @@ public class DriveTrain extends PIDSubsystem {
   }
 
   public void stop() {
-    drive(0, 0);
+    setMotorSpeeds(0, 0);
   }
 
   public void resetEncoders() {
@@ -169,7 +173,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 +192,19 @@ 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;
+    double k = (isFlipped() ? -1 : 1);
+    if (type == Constants.DriveTrain.TANK_DRIVE) {
+      robotDrive.tankDrive(-left * k, -right * k);
+    } else if (type == Constants.DriveTrain.ARCADE_DRIVE) {
+      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 = (Robot.driveTrain.isFlipped() ? -1 : 1);
+    robotDrive.tankDrive(-left * k, -right * k);
   }
 
   /**