| 1 | package org.usfirst.frc.team3501.robot.commandgroups; |
| 2 | |
| 3 | import org.usfirst.frc.team3501.robot.Constants.Direction; |
| 4 | import org.usfirst.frc.team3501.robot.commands.driving.DriveDistance; |
| 5 | import org.usfirst.frc.team3501.robot.commands.driving.TurnForAngle; |
| 6 | |
| 7 | import edu.wpi.first.wpilibj.command.CommandGroup; |
| 8 | |
| 9 | /** |
| 10 | * |
| 11 | */ |
| 12 | public class AutonShoot extends CommandGroup { |
| 13 | |
| 14 | private static final int ROBOT_WIDTH = 40; // inches |
| 15 | private static final int ROBOT_LENGTH = 36; // inches |
| 16 | |
| 17 | public AutonShoot() { |
| 18 | addSequential( |
| 19 | new DriveDistance(37.12 + (ROBOT_WIDTH / 2) - (ROBOT_LENGTH / 2), 5)); |
| 20 | addSequential(new TurnForAngle(135, Direction.LEFT, 10)); |
| 21 | addSequential(new DriveDistance( |
| 22 | (37.12 + (ROBOT_WIDTH / 2)) * Math.sqrt(2) - 26.25 - (ROBOT_LENGTH / 2), |
| 23 | 10)); |
| 24 | addSequential(new Shoot(15 - timeSinceInitialized())); |
| 25 | } |
| 26 | } |