change name of LowerChevalDeFrise to PassChevalDeFrise and add pre and post condition...

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

diff --git a/src/org/usfirst/frc/team3501/robot/commands/DefaultAutonStrategy.java b/src/org/usfirst/frc/team3501/robot/commands/DefaultAutonStrategy.java
index f2b07780e547181399ae0acc558d303c6d55b7eb..594e42faac2a93a80759ce23c8a35ca61aacfde9 100755 (executable)
--- a/src/org/usfirst/frc/team3501/robot/commands/DefaultAutonStrategy.java
+++ b/src/org/usfirst/frc/team3501/robot/commands/DefaultAutonStrategy.java
@@ -9,29 +9,69 @@ import edu.wpi.first.wpilibj.command.CommandGroup;
  * of it, aiming the robot/ shooter towards the goal, and shooting.
  */
 public class DefaultAutonStrategy extends CommandGroup {
+  // in feet
+  // distance along a platform
+  final double DIST_BETWEEN_OUTER_WORKS__AND_COURTYARD = 4.0;
+
+  // number from -1 to 1
+  final double SPEED_FOR_PASSING_DEFENSE = 0.5;
 
   public DefaultAutonStrategy(int position, Defense defense) {
-    /*
-     * pass defense drive forward do what has to be done to pass the defense
-     * drive forward aim face tower aim shooter toward goal shoot
-     */
 
-    // based on defense pass the defense
     switch (defense) {
+
     case PORTCULLIS:
+
+      addSequential(new LiftPortcullis());
+
     case SALLY_PORT:
+
+      addSequential(new PassSallyPort());
+
     case ROUGH_TERRAIN:
+
+      addSequential(
+          new DriveForDistance(DIST_BETWEEN_OUTER_WORKS__AND_COURTYARD + 2,
+              DIST_BETWEEN_OUTER_WORKS__AND_COURTYARD + 2));
+
     case LOW_BAR:
+
+      addSequential(
+          new DriveForDistance(DIST_BETWEEN_OUTER_WORKS__AND_COURTYARD + 2,
+              SPEED_FOR_PASSING_DEFENSE));
+
     case CHEVAL_DE_FRISE:
+
+      addSequential(new PassChevalDeFrise());
+
     case DRAWBRIDGE:
+
+      addSequential(new LowerDrawBridge());
+
     case MOAT:
+
+      addSequential(new DriveForDistance(
+          DIST_BETWEEN_OUTER_WORKS__AND_COURTYARD, SPEED_FOR_PASSING_DEFENSE));
+
     case ROCK_WALL:
+
+      addSequential(new DriveForDistance(
+          DIST_BETWEEN_OUTER_WORKS__AND_COURTYARD, SPEED_FOR_PASSING_DEFENSE));
+
     case RAMPART:
+
+      addSequential(new DriveForDistance(
+          DIST_BETWEEN_OUTER_WORKS__AND_COURTYARD, SPEED_FOR_PASSING_DEFENSE));
+
     default:
       break;
-
     }
 
+    // TODO: put in the argument that goes inside of aim, based on the position
+    // variable that was passed into the command group
+    addSequential(new Aim());
+    addSequential(new Shoot());
+
   }
 
 }