Add command group for ToggleWinch

[3501/2017steamworks] / src / org / usfirst / frc / team3501 / robot / commandgroups / ToggleWinch.java

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