make MoveToLevel

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

diff --git a/src/org/usfirst/frc3501/RiceCatRobot/commands/MoveToLevel.java b/src/org/usfirst/frc3501/RiceCatRobot/commands/MoveToLevel.java
new file mode 100644 (file)
index 0000000..ee10d1a
--- /dev/null
+++ b/src/org/usfirst/frc3501/RiceCatRobot/commands/MoveToLevel.java
@@ -0,0 +1,42 @@
+package org.usfirst.frc3501.RiceCatRobot.commands;
+
+import org.usfirst.frc3501.RiceCatRobot.Robot;
+import org.usfirst.frc3501.RiceCatRobot.RobotMap;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+public class MoveToLevel extends Command{
+
+       int targetLevel, currentLevel;
+       
+       public MoveToLevel(int targetLevel) {
+               this.targetLevel = targetLevel;
+       }
+       
+       @Override
+       protected void initialize() {
+               
+       }
+       
+       @Override
+       protected void execute() {
+               Robot.arm.setArmSpeeds(RobotMap.ARM_LOW_SPEED);
+       }
+
+       @Override
+       protected boolean isFinished() {
+               if(currentLevel == targetLevel) return true;
+               return false;
+       }       
+
+       @Override
+       protected void end() {
+               Robot.arm.setArmSpeeds(0);
+       }
+       
+       @Override
+       protected void interrupted() {
+               end();
+       }
+
+}