X-Git-Url: http://challenge-bot.com/repos/?a=blobdiff_plain;f=src%2Forg%2Fusfirst%2Ffrc%2Fteam3501%2Frobot%2Fcommandgroups%2FToggleWinch.java;fp=src%2Forg%2Fusfirst%2Ffrc%2Fteam3501%2Frobot%2Fcommandgroups%2FToggleWinch.java;h=3766aee271aa6d4028d773030054fde964af8621;hb=25ef99e622af7d7dbd7ddb6c9c1a7ec7e71b0b77;hp=0000000000000000000000000000000000000000;hpb=cef1f36da71950bc32d74d2ba0477a51f478f209;p=3501%2F2017steamworks

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
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());
+    }
+  }
+
+}