Commit | Line | Data |
---|---|---|
ee26ec15 ME |
1 | package org.usfirst.frc.team3501.robot.commands.intakearm; |
2 | ||
3 | import org.usfirst.frc.team3501.robot.Robot; | |
65424d4e | 4 | import org.usfirst.frc.team3501.robot.commands.driving.DriveDistance; |
ee26ec15 ME |
5 | |
6 | import edu.wpi.first.wpilibj.command.CommandGroup; | |
7 | ||
8 | /** | |
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 | |
11 | * | |
12 | * pre-condition: the robot is flush against the border of the outerworks (flush | |
13 | * against the ramp) | |
14 | * | |
15 | * post condition: intake arm has pushed the cheval de frise down | |
16 | * | |
17 | */ | |
18 | public class PushDownChevalDeFrise extends CommandGroup { | |
65424d4e ME |
19 | // distances are in inches |
20 | private final double MIN_ANGLE = 0; // TODO: find min angle the intake arm has | |
21 | // to be to be over the cheval de frise | |
22 | private final double RAMP_DIST = 12; | |
23 | private final double MAX_TIMEOUT_RAMP = 1; // TODO: check that one second is | |
24 | // enough | |
25 | private final double LOWEST_ANGLE = 0; // TODO: find angle | |
26 | private final double DEFAULT_ARM_SPEED = 0.4; | |
ee26ec15 ME |
27 | |
28 | public PushDownChevalDeFrise() { | |
29 | requires(Robot.intakeArm); | |
30 | /** | |
31 | * if the arm is high enough - above cheval de frise height (find this from | |
32 | * the potangle) | |
33 | * | |
34 | * go forward until arm is over cheval de frise | |
35 | * | |
36 | * move arm down to floor level -- or as much as possible | |
37 | * ------------------------------------------------------------------------- | |
38 | * | |
39 | * if the arm is beneath the cheval de frise | |
40 | * | |
41 | * move the arm up so it's high enough | |
42 | * | |
43 | * move forward a little | |
44 | * | |
65424d4e | 45 | * move arm down |
ee26ec15 ME |
46 | */ |
47 | ||
65424d4e ME |
48 | if (Robot.intakeArm.getArmAngle() > MIN_ANGLE) { |
49 | addSequential(new DriveDistance(RAMP_DIST, MAX_TIMEOUT_RAMP)); | |
50 | addSequential(new MoveIntakeArmToAngle(LOWEST_ANGLE, DEFAULT_ARM_SPEED)); | |
51 | ||
52 | } else { | |
53 | addSequential(new MoveIntakeArmToAngle(MIN_ANGLE + 3, DEFAULT_ARM_SPEED)); // TODO: | |
54 | // check that adding | |
55 | // 3 is correct | |
56 | ||
57 | addSequential(new DriveDistance(RAMP_DIST, MAX_TIMEOUT_RAMP)); | |
58 | addSequential(new MoveIntakeArmToAngle(LOWEST_ANGLE, DEFAULT_ARM_SPEED)); | |
59 | ||
60 | } | |
61 | ||
ee26ec15 ME |
62 | } |
63 | } |