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