delete anything to do with pots and motors in IntakeArm and Constants, add the left...

[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..9852a1c0cbc9c03fd1453a261d2483f6b413d72a 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,17 @@ import edu.wpi.first.wpilibj.command.CommandGroup;
  *
  */
 public class PushDownChevalDeFrise extends CommandGroup {
+  // distances are in inches
+  private final double MIN_ANGLE = Robot.intakeArm.potAngles[2]; // 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 = Robot.intakeArm.potAngles[0];
+  private final double DEFAULT_ARM_SPEED = 0.4;
 
   public PushDownChevalDeFrise() {
     requires(Robot.intakeArm);
@@ -33,7 +45,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));
+
+    }
 
   }
 }