Add rudimentary auton strategies based on timing

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

diff --git a/src/org/usfirst/frc/team3501/robot/commands/auton/ChooseStrategy.java b/src/org/usfirst/frc/team3501/robot/commands/auton/ChooseStrategy.java
new file mode 100644 (file)
index 0000000..50337c7
--- /dev/null
+++ b/src/org/usfirst/frc/team3501/robot/commands/auton/ChooseStrategy.java
@@ -0,0 +1,47 @@
+package org.usfirst.frc.team3501.robot.commands.auton;
+
+import org.usfirst.frc.team3501.robot.Constants.Auton;
+import org.usfirst.frc.team3501.robot.Constants.Defense;
+import org.usfirst.frc.team3501.robot.commands.shooter.Shoot;
+
+import edu.wpi.first.wpilibj.command.CommandGroup;
+import edu.wpi.first.wpilibj.command.WaitCommand;
+
+public class ChooseStrategy extends CommandGroup {
+
+  public ChooseStrategy(int position, Defense defense) {
+
+    if (defense == Defense.PORTCULLIS)
+      addSequential(new PassPortcullis());
+
+    else if (defense == Defense.CHIVAL_DE_FRISE)
+      addSequential(new PassChivalDeFrise());
+
+    else if (defense == Defense.MOAT)
+      addSequential(new PassMoat());
+
+    else if (defense == Defense.ROCK_WALL)
+      addSequential(new PassRockWall());
+
+    else if (defense == Defense.DRAWBRIDGE)
+      addSequential(new PassDrawbridge());
+
+    else if (defense == Defense.SALLY_PORT)
+      addSequential(new PassSallyPort());
+
+    else if (defense == Defense.RAMPART)
+      addSequential(new PassRampart());
+
+    else if (defense == Defense.ROUGH_TERRAIN)
+      addSequential(new PassRoughTerrain());
+
+    else if (defense == Defense.LOW_BAR)
+      addSequential(new PassLowBar());
+
+    addSequential(new AlignToScore(position));
+    // TODO: test for how long robot should wait
+    addSequential(new WaitCommand(Auton.WAIT_TIME));
+    addSequential(new Shoot());
+
+  }
+}