Commit | Line | Data |
---|---|---|
b70398a7 CZ |
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 | } |