implement PushDownChevalDeFrise command group

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

diff --git a/src/org/usfirst/frc/team3501/robot/commands/intakearm/PushDownChevalDeFrise.java b/src/org/usfirst/frc/team3501/robot/commands/intakearm/PushDownChevalDeFrise.java
index bccdc7d06844b01188e0a04a2214dc05b23feee3..f877cc64721cda24368244233111c76f27320ddc 100755 (executable)
--- a/src/org/usfirst/frc/team3501/robot/commands/intakearm/PushDownChevalDeFrise.java
+++ b/src/org/usfirst/frc/team3501/robot/commands/intakearm/PushDownChevalDeFrise.java
@@ -1,6 +1,7 @@
 package org.usfirst.frc.team3501.robot.commands.intakearm;
 
 import org.usfirst.frc.team3501.robot.Robot;
+import org.usfirst.frc.team3501.robot.commands.driving.DriveDistance;
 
 import edu.wpi.first.wpilibj.command.CommandGroup;
 
@@ -15,6 +16,14 @@ import edu.wpi.first.wpilibj.command.CommandGroup;
  *
  */
 public class PushDownChevalDeFrise extends CommandGroup {
+  // distances are in inches
+  private final double MIN_ANGLE = 0; // TODO: find min angle the intake arm has
+                                      // to be to be over the cheval de frise
+  private final double RAMP_DIST = 12;
+  private final double MAX_TIMEOUT_RAMP = 1; // TODO: check that one second is
+                                             // enough
+  private final double LOWEST_ANGLE = 0; // TODO: find angle
+  private final double DEFAULT_ARM_SPEED = 0.4;
 
   public PushDownChevalDeFrise() {
     requires(Robot.intakeArm);
@@ -33,7 +42,22 @@ public class PushDownChevalDeFrise extends CommandGroup {
      *
      * move forward a little
      *
+     * move arm down
      */
 
+    if (Robot.intakeArm.getArmAngle() > MIN_ANGLE) {
+      addSequential(new DriveDistance(RAMP_DIST, MAX_TIMEOUT_RAMP));
+      addSequential(new MoveIntakeArmToAngle(LOWEST_ANGLE, DEFAULT_ARM_SPEED));
+
+    } else {
+      addSequential(new MoveIntakeArmToAngle(MIN_ANGLE + 3, DEFAULT_ARM_SPEED)); // TODO:
+      // check that adding
+      // 3 is correct
+
+      addSequential(new DriveDistance(RAMP_DIST, MAX_TIMEOUT_RAMP));
+      addSequential(new MoveIntakeArmToAngle(LOWEST_ANGLE, DEFAULT_ARM_SPEED));
+
+    }
+
   }
 }