Implement team 254's 2014 driving code

[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 a0134a43a7fbe0d6c792924c1da98dabff741c14..1079d501a82fc4f52d281b0c8e96478a2a0653a9 100644 (file)
--- a/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java
+++ b/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java
@@ -1,5 +1,6 @@
 package org.usfirst.frc.team3501.robot.subsystems;
 
+import org.usfirst.frc.team3501.robot.CheesyDriveHelper;
 import org.usfirst.frc.team3501.robot.Constants;
 import org.usfirst.frc.team3501.robot.commands.driving.JoystickDrive;
 
@@ -24,6 +25,8 @@ public class DriveTrain extends PIDSubsystem {
 
   private DoubleSolenoid leftGearPiston, rightGearPiston;
 
+  private CheesyDriveHelper cheese;
+
   // Drivetrain specific constants that relate to the inches per pulse value for
   // the encoders
 
@@ -57,6 +60,8 @@ public class DriveTrain extends PIDSubsystem {
         Constants.DriveTrain.RIGHT_SHIFT_MODULE,
         Constants.DriveTrain.RIGHT_SHIFT_FORWARD,
         Constants.DriveTrain.RIGHT_SHIFT_REVERSE);
+
+    cheese = new CheesyDriveHelper(this);
   }
 
   @Override
@@ -196,13 +201,14 @@ public class DriveTrain extends PIDSubsystem {
   public void joystickDrive(double left, double right) {
     // Handle flipping of the "front" of the robot
     double k = (isFlipped() ? -1 : 1);
-
-    robotDrive.tankDrive(-left * k, -right * k);
+    cheese.cheesyDrive(-left * k, -right,
+        getGearPistonValue() == Constants.DriveTrain.HIGH_GEAR);
   }
 
   public void setMotorSpeeds(double left, double right) {
     double k = (isFlipped() ? -1 : 1);
-    robotDrive.tankDrive(-left * k, -right * k);
+    cheese.cheesyDrive(-left * k, -right,
+        getGearPistonValue() == Constants.DriveTrain.HIGH_GEAR);
   }
 
   /**