Uses arrays and sends message when a specific limitswitch is hit

[3501/2015-FRC-Spark] / src / org / usfirst / frc3501 / RiceCatRobot / commands / MoveArmToLevel3.java

diff --git a/src/org/usfirst/frc3501/RiceCatRobot/commands/MoveArmToLevel3.java b/src/org/usfirst/frc3501/RiceCatRobot/commands/MoveArmToLevel3.java
new file mode 100644 (file)
index 0000000..d969dee
--- /dev/null
+++ b/src/org/usfirst/frc3501/RiceCatRobot/commands/MoveArmToLevel3.java
@@ -0,0 +1,37 @@
+package org.usfirst.frc3501.RiceCatRobot.commands;
+
+import org.usfirst.frc3501.RiceCatRobot.subsystems.Arm;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+public class MoveArmToLevel3 extends Command {
+       Arm arm;
+
+       double slowSpeed = 0.2;
+
+       public MoveArmToLevel3() {
+               if (arm.getArmSpeed() == 0.0) {
+                       arm.setArmSpeeds(slowSpeed);
+               }
+       }
+
+       protected void initialize() {
+               arm.initializeCounters();
+       }
+
+       protected void execute() {
+       }
+
+       protected boolean isFinished() {
+               return arm.isSwitch3Hit();
+       }
+
+       protected void end() {
+               System.out.println("Robot arm has reached level 3");
+               arm.stop();
+       }
+
+       protected void interrupted() {
+               end();
+       }
+}