| 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 | } |