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 | 19 | // distances are in inches |
7952bc91 ME |
20 | private final double MIN_ANGLE = Robot.intakeArm.potAngles[2]; // TODO: find |
21 | // min angle | |
22 | // the intake | |
23 | // arm has | |
24 | // to be to be over the cheval de frise | |
65424d4e ME |
25 | private final double RAMP_DIST = 12; |
26 | private final double MAX_TIMEOUT_RAMP = 1; // TODO: check that one second is | |
27 | // enough | |
7952bc91 | 28 | private final double LOWEST_ANGLE = Robot.intakeArm.potAngles[0]; |
65424d4e | 29 | private final double DEFAULT_ARM_SPEED = 0.4; |
ee26ec15 ME |
30 | |
31 | public PushDownChevalDeFrise() { | |
32 | requires(Robot.intakeArm); | |
33 | /** | |
34 | * if the arm is high enough - above cheval de frise height (find this from | |
35 | * the potangle) | |
36 | * | |
37 | * go forward until arm is over cheval de frise | |
38 | * | |
39 | * move arm down to floor level -- or as much as possible | |
40 | * ------------------------------------------------------------------------- | |
41 | * | |
42 | * if the arm is beneath the cheval de frise | |
43 | * | |
44 | * move the arm up so it's high enough | |
45 | * | |
46 | * move forward a little | |
47 | * | |
65424d4e | 48 | * move arm down |
7952bc91 | 49 | **/ |
ee26ec15 | 50 | |
65424d4e ME |
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)); | |
54 | ||
55 | } else { | |
56 | addSequential(new MoveIntakeArmToAngle(MIN_ANGLE + 3, DEFAULT_ARM_SPEED)); // TODO: | |
57 | // check that adding | |
58 | // 3 is correct | |
59 | ||
60 | addSequential(new DriveDistance(RAMP_DIST, MAX_TIMEOUT_RAMP)); | |
61 | addSequential(new MoveIntakeArmToAngle(LOWEST_ANGLE, DEFAULT_ARM_SPEED)); | |
62 | ||
63 | } | |
64 | ||
ee26ec15 ME |
65 | } |
66 | } |