Commit | Line | Data |
---|---|---|
e6e87119 A |
1 | |
2 | package org.usfirst.frc.team3501.robot.commandgroups; | |
3 | ||
4 | import org.usfirst.frc.team3501.robot.Constants.Direction; | |
5 | import org.usfirst.frc.team3501.robot.commands.driving.DriveDistance; | |
6 | import org.usfirst.frc.team3501.robot.commands.driving.TurnForAngle; | |
7 | ||
8 | import edu.wpi.first.wpilibj.command.CommandGroup; | |
9 | ||
10 | /** | |
11 | * | |
12 | * Authors Ayush, Nadia, Aziza and Abhinav | |
13 | * | |
14 | * This comandGroup, PlaceGearOnMiddlePeg is expected to have the robot start | |
15 | * from the middle of the starting line, right in front of the airship. The | |
16 | * robot will drive forward and place the gear on the peg. Then, the robot will | |
17 | * drive backwards, turn left, drive forward, turn right, and drive forward | |
18 | * again to cross the baseline. | |
19 | */ | |
20 | public class AutonMiddleGear extends CommandGroup { | |
21 | private static final double DISTANCE_TO_PEG = 91.3; | |
22 | private static final double DISTANCE_TO_BACK_OUT = 29.75; | |
23 | private static final double THIRD_DISTANCE_TO_TRAVEL = 70; | |
24 | private static final double DISTANCE_TO_BASELINE = 50.5; | |
25 | ||
26 | private static final double ANGLE_TO_TURN = 90; | |
27 | ||
28 | private static final double maxTimeOut = 7; | |
29 | ||
30 | /*** | |
31 | * This auton command group places the gear on the middle peg then crosses the | |
32 | * baseline | |
33 | * | |
34 | * @param direction | |
35 | * direction to turn after placing gear on peg in order to cross the | |
36 | * baseline. Only Direction.LEFT and Direction.RIGHT will be accepted | |
37 | */ | |
38 | public AutonMiddleGear(Direction direction) { | |
39 | addSequential(new DriveDistance(DISTANCE_TO_PEG, maxTimeOut)); | |
40 | addSequential(new DriveDistance(DISTANCE_TO_BACK_OUT, maxTimeOut)); | |
41 | addSequential(new TurnForAngle(ANGLE_TO_TURN, direction, maxTimeOut)); | |
42 | addSequential(new DriveDistance(THIRD_DISTANCE_TO_TRAVEL, maxTimeOut)); | |
43 | addSequential( | |
44 | new TurnForAngle(ANGLE_TO_TURN, oppositeOf(direction), maxTimeOut)); | |
45 | addSequential(new DriveDistance(DISTANCE_TO_BASELINE, maxTimeOut)); | |
46 | } | |
47 | ||
48 | private Direction oppositeOf(Direction direction) { | |
49 | if (direction == Direction.LEFT) | |
50 | return Direction.RIGHT; | |
51 | return Direction.LEFT; | |
52 | } | |
53 | } |