Add command group for ToggleWinch

diff --git a/src/org/usfirst/frc/team3501/robot/OI.java b/src/org/usfirst/frc/team3501/robot/OI.java
index 32ec06253e7be998195641cdaf87c114c52f935d..05fb72dbbbf46fc6c1bad4c9df8adbddc679354b 100644 (file)
--- a/src/org/usfirst/frc/team3501/robot/OI.java
+++ b/src/org/usfirst/frc/team3501/robot/OI.java
@@ -1,7 +1,6 @@
 package org.usfirst.frc.team3501.robot;
 
-import org.usfirst.frc.team3501.robot.commands.climber.MaintainClimbedPosition;
-import org.usfirst.frc.team3501.robot.commands.climber.RunWinchContinuous;
+import org.usfirst.frc.team3501.robot.commandgroups.ToggleWinch;
 import org.usfirst.frc.team3501.robot.commands.driving.ToggleGear;
 import org.usfirst.frc.team3501.robot.commands.intake.ReverseIntakeContinuous;
 import org.usfirst.frc.team3501.robot.commands.intake.RunIntakeContinuous;
@@ -18,7 +17,6 @@ public class OI {
   public static Joystick leftJoystick;
   public static Joystick rightJoystick;
   public static Button toggleWinch;
-  private boolean isClimbing = false;
 
   public static Button runIndexWheel;
   public static Button reverseIndexWheel;
@@ -59,13 +57,13 @@ public class OI {
 
     toggleWinch = new JoystickButton(leftJoystick,
         Constants.OI.TOGGLE_WINCH_PORT);
-    if (!isClimbing) {
-      toggleWinch.whenPressed(new RunWinchContinuous());
-      isClimbing = true;
-    } else {
-      toggleWinch.whenPressed(new MaintainClimbedPosition());
-      isClimbing = false;
-    }
+    /*
+     * if (!Robot.getDriveTrain().isClimbing()) { toggleWinch.whenPressed(new
+     * RunWinchContinuous()); Robot.getDriveTrain().setClimbing(true); } else {
+     * toggleWinch.whenPressed(new MaintainClimbedPosition());
+     * Robot.getDriveTrain().setClimbing(false); }
+     */
+    toggleWinch.whenPressed(new ToggleWinch());
   }
 
   public static OI getOI() {

diff --git a/src/org/usfirst/frc/team3501/robot/commandgroups/ToggleWinch.java b/src/org/usfirst/frc/team3501/robot/commandgroups/ToggleWinch.java
new file mode 100644 (file)
index 0000000..3766aee
--- /dev/null
+++ b/src/org/usfirst/frc/team3501/robot/commandgroups/ToggleWinch.java
@@ -0,0 +1,22 @@
+package org.usfirst.frc.team3501.robot.commandgroups;
+
+import org.usfirst.frc.team3501.robot.Robot;
+import org.usfirst.frc.team3501.robot.commands.climber.MaintainClimbedPosition;
+import org.usfirst.frc.team3501.robot.commands.climber.RunWinchContinuous;
+
+import edu.wpi.first.wpilibj.command.CommandGroup;
+
+public class ToggleWinch extends CommandGroup {
+
+  public ToggleWinch() {
+    if (!Robot.getDriveTrain().isClimbing()) {
+      Robot.getDriveTrain().setClimbing(true);
+      addSequential(new RunWinchContinuous());
+
+    } else {
+      Robot.getDriveTrain().setClimbing(false);
+      addSequential(new MaintainClimbedPosition());
+    }
+  }
+
+}

diff --git a/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java b/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java
index f30d4c2d2d41c8e62fd4b01e78cd054e75e8b89a..7463b8685faa504319777e830231c066b8088154 100644 (file)
--- a/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java
+++ b/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java
@@ -81,6 +81,7 @@ public class DriveTrain extends Subsystem {
 
     frontRight.set(-right);
     rearRight.set(-right);
+    this.isClimbing = true;
   }
 
   public void joystickDrive(final double thrust, final double twist) {
@@ -89,6 +90,7 @@ public class DriveTrain extends Subsystem {
 
   public void stop() {
     setMotorValues(0, 0);
+    this.isClimbing = false;
   }
 
   public double getLeftMotorVal() {
@@ -192,6 +194,10 @@ public class DriveTrain extends Subsystem {
     return this.isClimbing;
   }
 
+  public void setClimbing(boolean isClimbing) {
+    this.isClimbing = isClimbing;
+  }
+
   public double getClimbingSpeed() {
     return this.CLIMBER_SPEED;
   }