From 13eda6855c76fed9ed693e08b62a0424b92c2c12 Mon Sep 17 00:00:00 2001
From: Harel Dor <hareldor@gmail.com>
Date: Sun, 21 Feb 2016 21:09:39 -0800
Subject: [PATCH] Add forgetten things from previous commit. Fix this

---
 .../robot/commands/driving/ChangeGear.java    | 45 +++++++++++++++++++
 .../commands/driving/ToggleCompressor.java    | 38 ++++++++++++++++
 .../team3501/robot/subsystems/DriveTrain.java |  2 +-
 3 files changed, 84 insertions(+), 1 deletion(-)
 create mode 100644 src/org/usfirst/frc/team3501/robot/commands/driving/ChangeGear.java
 create mode 100644 src/org/usfirst/frc/team3501/robot/commands/driving/ToggleCompressor.java

diff --git a/src/org/usfirst/frc/team3501/robot/commands/driving/ChangeGear.java b/src/org/usfirst/frc/team3501/robot/commands/driving/ChangeGear.java
new file mode 100644
index 00000000..3f613dc9
--- /dev/null
+++ b/src/org/usfirst/frc/team3501/robot/commands/driving/ChangeGear.java
@@ -0,0 +1,45 @@
+package org.usfirst.frc.team3501.robot.commands.driving;
+
+import org.usfirst.frc.team3501.robot.Constants;
+import org.usfirst.frc.team3501.robot.Robot;
+
+import edu.wpi.first.wpilibj.DoubleSolenoid.Value;
+import edu.wpi.first.wpilibj.command.Command;
+
+/**
+ *
+ */
+public class ChangeGear extends Command {
+
+  public ChangeGear() {
+    // Doesn't require drivetrain because change can be made while robot is
+    // driving
+  }
+
+  @Override
+  protected void initialize() {
+    Value gear = Robot.driveTrain.getLeftGearPistonValue(); // Gears are assumed
+                                                            // to be the same
+    Value opposite = (gear == Constants.DriveTrain.HIGH_GEAR) ? Constants.DriveTrain.LOW_GEAR
+        : Constants.DriveTrain.HIGH_GEAR;
+    Robot.driveTrain.changeGear(opposite);
+  }
+
+  @Override
+  protected void execute() {
+  }
+
+  @Override
+  protected boolean isFinished() {
+    return true;
+  }
+
+  @Override
+  protected void end() {
+  }
+
+  @Override
+  protected void interrupted() {
+    end();
+  }
+}
diff --git a/src/org/usfirst/frc/team3501/robot/commands/driving/ToggleCompressor.java b/src/org/usfirst/frc/team3501/robot/commands/driving/ToggleCompressor.java
new file mode 100644
index 00000000..f78acb25
--- /dev/null
+++ b/src/org/usfirst/frc/team3501/robot/commands/driving/ToggleCompressor.java
@@ -0,0 +1,38 @@
+package org.usfirst.frc.team3501.robot.commands.driving;
+
+import org.usfirst.frc.team3501.robot.Robot;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+/**
+ *
+ */
+public class ToggleCompressor extends Command {
+
+  public ToggleCompressor() {
+
+  }
+
+  @Override
+  protected void initialize() {
+    Robot.driveTrain.toggleCompressor();
+  }
+
+  @Override
+  protected void execute() {
+  }
+
+  @Override
+  protected boolean isFinished() {
+    return true;
+  }
+
+  @Override
+  protected void end() {
+  }
+
+  @Override
+  protected void interrupted() {
+    end();
+  }
+}
diff --git a/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java b/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java
index f5a7d6c2..064c1894 100644
--- a/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java
+++ b/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java
@@ -253,7 +253,7 @@ public class DriveTrain extends PIDSubsystem {
    * because RobotDrive tankdrive method drives inverted
    */
   public void drive(double left, double right) {
-//    robotDrive.tankDrive(-left, -right);
+    // robotDrive.tankDrive(-left, -right);
     // dunno why but inverted drive (- values is forward)
     if (!Constants.DriveTrain.inverted)
       robotDrive.tankDrive(-left, -right);
-- 
2.30.2