add code for switching b/w cameras

[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 0f52e115d8eda7dca2cfbf7b04cc8a49fb08084b..807bb151ec2d3b239329c3f9aafeabd41ff438f8 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
@@ -159,10 +164,10 @@ public class DriveTrain extends PIDSubsystem {
   /*
    * 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
@@ -185,7 +190,7 @@ public class DriveTrain extends PIDSubsystem {
 
   /*
    * 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
    */
@@ -194,35 +199,23 @@ public class DriveTrain extends PIDSubsystem {
   }
 
   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);
-
-    robotDrive.tankDrive(-left, -right);
-
-    // 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);
-    // }
+    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);
+     */
+
+    frontLeft.set(left);
+    rearLeft.set(left);
+    frontRight.set(right);
+    rearRight.set(right);
   }
 
   /**
@@ -260,8 +253,8 @@ public class DriveTrain extends PIDSubsystem {
    */
   public void switchGear() {
     Value currentValue = getGearPistonValue();
-    Value setValue = (currentValue == Constants.DriveTrain.HIGH_GEAR) ? Constants.DriveTrain.LOW_GEAR
-        : Constants.DriveTrain.HIGH_GEAR;
+    Value setValue = (currentValue == Constants.DriveTrain.HIGH_GEAR)
+        ? Constants.DriveTrain.LOW_GEAR : Constants.DriveTrain.HIGH_GEAR;
     changeGear(setValue);
   }