Recode everything for new robot

[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 5fa529335b57c5315c3000e9ffbc89d734ae17b3..dec42e9d356cee52b6c43d2087093b1811476167 100644 (file)
--- a/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java
+++ b/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java
@@ -18,6 +18,7 @@ import edu.wpi.first.wpilibj.command.PIDSubsystem;
 public class DriveTrain extends PIDSubsystem {
   // Current Drive Mode Default Drive Mode is Manual
   private int DRIVE_MODE = 1;
+  private boolean outputFlipped = false;
   private static double pidOutput = 0;
 
   private Encoder leftEncoder, rightEncoder;
@@ -62,11 +63,10 @@ public class DriveTrain extends PIDSubsystem {
     this.disable();
     gyro.start();
 
-    leftGearPiston = new DoubleSolenoid(10, Constants.DriveTrain.LEFT_FORWARD,
-        Constants.DriveTrain.LEFT_REVERSE);
-    rightGearPiston = new DoubleSolenoid(10,
-        Constants.DriveTrain.RIGHT_FORWARD,
-        Constants.DriveTrain.RIGHT_REVERSE);
+    leftGearPiston = new DoubleSolenoid(Constants.DriveTrain.LEFT_MODULE,
+        Constants.DriveTrain.LEFT_FORWARD, Constants.DriveTrain.LEFT_REVERSE);
+    rightGearPiston = new DoubleSolenoid(Constants.DriveTrain.RIGHT_MODULE,
+        Constants.DriveTrain.RIGHT_FORWARD, Constants.DriveTrain.RIGHT_REVERSE);
     Constants.DriveTrain.inverted = false;
   }
 
@@ -197,10 +197,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
@@ -228,7 +228,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
    */
@@ -260,9 +260,9 @@ public class DriveTrain extends PIDSubsystem {
   /*
    * constrains the distance to within -100 and 100 since we aren't going to
    * drive more than 100 inches
-   *
+   * 
    * Configure Encoder PID
-   *
+   * 
    * Sets the setpoint to the PID subsystem
    */
   public void driveDistance(double dist, double maxTimeOut) {
@@ -302,10 +302,10 @@ public class DriveTrain extends PIDSubsystem {
 
   /*
    * Turning method that should be used repeatedly in a command
-   *
+   * 
    * First constrains the angle to within -360 and 360 since that is as much as
    * we need to turn
-   *
+   * 
    * Configures Gyro PID and sets the setpoint as an angle
    */
   public void turnAngle(double angle) {
@@ -323,42 +323,56 @@ public class DriveTrain extends PIDSubsystem {
     rearRight.set(right);
   }
 
-  /*
-   * @return a value that is the current setpoint for the piston kReverse or
-   * kForward
-   */
-  public Value getLeftGearPistonValue() {
-    return leftGearPiston.get();
-  }
-
-  /*
-   * @return a value that is the current setpoint for the piston kReverse or
-   * kForward
+  /**
+   * @return a value that is the current setpoint for the piston (kReverse or
+   *         kForward)
    */
-  public Value getRightGearPistonValue() {
-    return rightGearPiston.get();
+  public Value getGearPistonValue() {
+    return leftGearPiston.get(); // Pistons should always be in the same state
   }
 
-  /*
+  /**
    * Changes the ball shift gear assembly to high
    */
   public void setHighGear() {
     changeGear(Constants.DriveTrain.HIGH_GEAR);
   }
 
-  /*
+  /**
    * Changes the ball shift gear assembly to low
    */
   public void setLowGear() {
     changeGear(Constants.DriveTrain.LOW_GEAR);
   }
 
-  /*
-   * changes the gear to a DoubleSolenoid.Value
+  /**
+   * Changes the gear to a DoubleSolenoid.Value
    */
   public void changeGear(DoubleSolenoid.Value gear) {
     leftGearPiston.set(gear);
     rightGearPiston.set(gear);
   }
 
+  /**
+   * Switches drivetrain gears from high to low or low to high
+   */
+  public void switchGear() {
+    Value currentValue = getGearPistonValue();
+    Value setValue = (currentValue == Constants.DriveTrain.HIGH_GEAR) ? Constants.DriveTrain.LOW_GEAR
+        : Constants.DriveTrain.HIGH_GEAR;
+    changeGear(setValue);
+  }
+
+  /**
+   * Toggle whether the motor outputs are flipped, effectively switching which
+   * side of the robot is the front.
+   */
+  public void toggleFlipped() {
+    outputFlipped = !outputFlipped;
+  }
+
+  public boolean isFlipped() {
+    return outputFlipped;
+  }
+
 }