add claw toggle and winch tensioning

[3501/3501-spark-go] / src / org / usfirst / frc / team3501 / robot / commands / ToggleClaw.java

diff --git a/src/org/usfirst/frc/team3501/robot/commands/ToggleClaw.java b/src/org/usfirst/frc/team3501/robot/commands/ToggleClaw.java
new file mode 100644 (file)
index 0000000..f1afa16
--- /dev/null
+++ b/src/org/usfirst/frc/team3501/robot/commands/ToggleClaw.java
@@ -0,0 +1,20 @@
+package org.usfirst.frc.team3501.robot.commands;
+
+public class ToggleClaw extends CommandBase {
+
+    public ToggleClaw() {
+        super("ToggleClaw");
+        requires(claw);
+    }
+
+    protected void initialize() {
+        if (claw.isOpen())
+            schedule(new CloseClaw());
+        else
+            schedule(new OpenClaw());
+    }
+
+    protected boolean isFinished() {
+        return true;
+    }
+}