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;
*
*/
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);
*
* 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));
+
+ }
+
}
}