1 package org
.usfirst
.frc
.team3501
.robot
.commands
.intakearm
;
3 import org
.usfirst
.frc
.team3501
.robot
.Robot
;
4 import org
.usfirst
.frc
.team3501
.robot
.commands
.driving
.DriveDistance
;
6 import edu
.wpi
.first
.wpilibj
.command
.CommandGroup
;
9 * This command group uses the intake arm to push down the moving parts of the
10 * cheval de frise so that the robot can drive over it
12 * pre-condition: the robot is flush against the border of the outerworks (flush
15 * post condition: intake arm has pushed the cheval de frise down
18 public class PushDownChevalDeFrise
extends CommandGroup
{
19 // distances are in inches
20 private final double MIN_ANGLE
= Robot
.intakeArm
.potAngles
[2]; // TODO: find
24 // to be to be over the cheval de frise
25 private final double RAMP_DIST
= 12;
26 private final double MAX_TIMEOUT_RAMP
= 1; // TODO: check that one second is
28 private final double LOWEST_ANGLE
= Robot
.intakeArm
.potAngles
[0];
29 private final double DEFAULT_ARM_SPEED
= 0.4;
31 public PushDownChevalDeFrise() {
32 requires(Robot
.intakeArm
);
34 * if the arm is high enough - above cheval de frise height (find this from
37 * go forward until arm is over cheval de frise
39 * move arm down to floor level -- or as much as possible
40 * -------------------------------------------------------------------------
42 * if the arm is beneath the cheval de frise
44 * move the arm up so it's high enough
46 * move forward a little
51 if (Robot
.intakeArm
.getArmAngle() > MIN_ANGLE
) {
52 addSequential(new DriveDistance(RAMP_DIST
, MAX_TIMEOUT_RAMP
));
53 addSequential(new MoveIntakeArmToAngle(LOWEST_ANGLE
, DEFAULT_ARM_SPEED
));
56 addSequential(new MoveIntakeArmToAngle(MIN_ANGLE
+ 3, DEFAULT_ARM_SPEED
)); // TODO:
60 addSequential(new DriveDistance(RAMP_DIST
, MAX_TIMEOUT_RAMP
));
61 addSequential(new MoveIntakeArmToAngle(LOWEST_ANGLE
, DEFAULT_ARM_SPEED
));