| 1 | package org.usfirst.frc.team3501.robot.commands.intakearm; |
| 2 | |
| 3 | import org.usfirst.frc.team3501.robot.Robot; |
| 4 | import org.usfirst.frc.team3501.robot.commands.driving.DriveDistance; |
| 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 { |
| 19 | // distances are in inches |
| 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 |
| 25 | private final double RAMP_DIST = 12; |
| 26 | private final double MAX_TIMEOUT_RAMP = 1; // TODO: check that one second is |
| 27 | // enough |
| 28 | private final double LOWEST_ANGLE = Robot.intakeArm.potAngles[0]; |
| 29 | private final double DEFAULT_ARM_SPEED = 0.4; |
| 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 | * |
| 48 | * move arm down |
| 49 | **/ |
| 50 | |
| 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 | |
| 65 | } |
| 66 | } |