Commit | Line | Data |
---|---|---|
b70398a7 CZ |
1 | package org.usfirst.frc.team3501.robot.commandgroups; |
2 | ||
3 | import org.usfirst.frc.team3501.robot.Constants; | |
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.Timer; | |
9 | import edu.wpi.first.wpilibj.command.CommandGroup; | |
10 | import edu.wpi.first.wpilibj.command.WaitCommand; | |
11 | ||
12 | /** | |
13 | * // Robot starts in middle, goes to the hopper, then boiler,then shoots during | |
14 | * auton | |
15 | */ | |
16 | public class AutonHopperShoot extends CommandGroup { | |
17 | // If red, direction is right; if blue, direction is left | |
18 | private static final Direction DIRECTION_TO_HOPPER = Constants.Direction.LEFT; | |
19 | // If red, direction is left; if blue, direction is right | |
20 | private static final Direction DIRECTION_TO_BOILER = Constants.Direction.RIGHT; | |
21 | ||
22 | private Timer timer; | |
23 | ||
24 | public AutonHopperShoot() { | |
25 | timer = new Timer(); | |
26 | // Robot drives from center to front of airship | |
27 | addSequential(new DriveDistance(78.5, 2.7)); | |
28 | // Robot turns towards hopper | |
29 | addSequential(new TurnForAngle(90.0, DIRECTION_TO_HOPPER, 2.5)); | |
30 | // Robot drives into hopper switch | |
31 | addSequential(new DriveDistance(42.12, 5.25)); | |
32 | addSequential(new WaitCommand(1)); | |
33 | // Robot backs up from switch | |
34 | addSequential(new DriveDistance(-25.0, 2.9)); | |
35 | // Robot turns towards the boiler | |
36 | addSequential(new TurnForAngle(90.0, DIRECTION_TO_HOPPER, 5.0)); | |
37 | // Robot drives to boiler | |
38 | addSequential(new DriveDistance(90, 5.0)); | |
39 | // Robot turns parallel to boiler | |
40 | addSequential(new TurnForAngle(45, DIRECTION_TO_BOILER, 5.0)); | |
41 | // Shoot | |
42 | addSequential(new Shoot(15 - timeSinceInitialized())); | |
43 | } | |
44 | ||
45 | } |