implement MoveIntakeArmToLevel command group

[3501/stronghold-2016] / src / org / usfirst / frc / team3501 / robot / commands / intakearm / MoveIntakeArmToLevel.java

diff --git a/src/org/usfirst/frc/team3501/robot/commands/intakearm/MoveIntakeArmToLevel.java b/src/org/usfirst/frc/team3501/robot/commands/intakearm/MoveIntakeArmToLevel.java
new file mode 100755 (executable)
index 0000000..9943bcd
--- /dev/null
+++ b/src/org/usfirst/frc/team3501/robot/commands/intakearm/MoveIntakeArmToLevel.java
@@ -0,0 +1,30 @@
+package org.usfirst.frc.team3501.robot.commands.intakearm;
+
+import org.usfirst.frc.team3501.robot.Robot;
+
+import edu.wpi.first.wpilibj.command.CommandGroup;
+
+/**
+ * This command group will move the arm to the specified level. Levels
+ * correspond to the angles stored in the potAngles array list saved in the
+ * IntakeArm subsystem. (starting from level 0)
+ *
+ * pre-condition: level >= 0 && level < potAngles.length
+ *
+ * post-condition: arm has moved to specified level
+ *
+ * @author Meryem
+ */
+public class MoveIntakeArmToLevel extends CommandGroup {
+
+  public MoveIntakeArmToLevel(int level) {
+    requires(Robot.intakeArm);
+
+    if (level < 0 || level >= Robot.intakeArm.potAngles.length)
+      this.end();
+
+    addSequential(
+        new MoveIntakeArmToAngle(Robot.intakeArm.potAngles[level], 0));
+
+  }
+}