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