Commit | Line | Data |
---|---|---|
0b403e60 ME |
1 | package org.usfirst.frc.team3501.robot.commands; |
2 | ||
93453536 CZ |
3 | import org.usfirst.frc.team3501.robot.commands.driving.DriveDistance; |
4 | import org.usfirst.frc.team3501.robot.commands.driving.TurnForAngle; | |
0b403e60 ME |
5 | |
6 | import edu.wpi.first.wpilibj.command.CommandGroup; | |
7 | ||
8 | /** | |
9 | * This command group will be used in autonomous. Based on what position the | |
10 | * robot is in, the robot will align with the goal | |
11 | * | |
12 | * pre-condition: robot is flush against a defense at the specified position in | |
13 | * the opponent's courtyard | |
14 | * | |
15 | * post-condition: the robot is parallel to one of the three goals and the | |
16 | * shooter is facing that goal | |
17 | * | |
18 | */ | |
19 | public class AlignToScore extends CommandGroup { | |
20 | private final double DIST_CENTER_OF_MASS_TO_FRONT_OF_ROBOT = 0; | |
21 | ||
22 | private final double DEFAULT_SPEED = 0.5; | |
93453536 | 23 | private final double maxTimeout = 5; |
0b403e60 ME |
24 | |
25 | // constants for position 1: low bar | |
26 | private final double POS1_DIST1 = 0; | |
27 | private final double POS1_TURN1 = 0; | |
28 | private final double POS1_DIST2 = 0; | |
29 | ||
30 | // constants for position 2 | |
31 | private final double POS2_DIST1 = 0; | |
32 | private final double POS2_TURN1 = 0; | |
33 | private final double POS2_DIST2 = 0; | |
34 | ||
35 | // constants for position 3 | |
36 | private final double POS3_DIST1 = 0; | |
37 | private final double POS3_TURN1 = 0; | |
38 | private final double POS3_DIST2 = 0; | |
39 | private final double POS3_TURN2 = 0; | |
40 | private final double POS3_DIST3 = 0; | |
41 | ||
42 | // constants for position 4 | |
43 | private final double POS4_DIST1 = 0; | |
44 | private final double POS4_TURN1 = 0; | |
45 | private final double POS4_DIST2 = 0; | |
46 | private final double POS4_TURN2 = 0; | |
47 | private final double POS4_DIST3 = 0; | |
48 | ||
49 | // constants for position 5 | |
50 | private final double POS5_DIST1 = 0; | |
51 | private final double POS5_TURN1 = 0; | |
52 | private final double POS5_DIST2 = 0; | |
53 | ||
54 | public AlignToScore(int position) { | |
55 | ||
56 | switch (position) { | |
57 | ||
58 | // position 1 is always the low bar | |
59 | case 1: | |
60 | ||
93453536 CZ |
61 | addSequential(new DriveDistance(POS1_DIST1, DEFAULT_SPEED)); |
62 | addSequential(new TurnForAngle(POS1_TURN1, maxTimeout)); | |
63 | addSequential(new DriveDistance(POS1_DIST2, DEFAULT_SPEED)); | |
0b403e60 ME |
64 | |
65 | case 2: | |
66 | ||
93453536 CZ |
67 | addSequential(new DriveDistance(POS2_DIST1, DEFAULT_SPEED)); |
68 | addSequential(new TurnForAngle(POS2_TURN1, maxTimeout)); | |
69 | addSequential(new DriveDistance(POS2_DIST2, DEFAULT_SPEED)); | |
0b403e60 ME |
70 | |
71 | case 3: | |
72 | ||
93453536 CZ |
73 | addSequential(new DriveDistance(POS3_DIST1, DEFAULT_SPEED)); |
74 | addSequential(new TurnForAngle(POS3_TURN1, maxTimeout)); | |
75 | addSequential(new DriveDistance(POS3_DIST2, DEFAULT_SPEED)); | |
76 | addSequential(new TurnForAngle(POS3_TURN2, maxTimeout)); | |
77 | addSequential(new DriveDistance(POS3_DIST3, DEFAULT_SPEED)); | |
0b403e60 ME |
78 | |
79 | case 4: | |
80 | ||
93453536 CZ |
81 | addSequential(new DriveDistance(POS4_DIST1, DEFAULT_SPEED)); |
82 | addSequential(new TurnForAngle(POS4_TURN1, maxTimeout)); | |
83 | addSequential(new DriveDistance(POS4_DIST2, DEFAULT_SPEED)); | |
84 | addSequential(new TurnForAngle(POS4_TURN2, maxTimeout)); | |
85 | addSequential(new DriveDistance(POS4_DIST3, DEFAULT_SPEED)); | |
0b403e60 ME |
86 | |
87 | case 5: | |
88 | ||
93453536 CZ |
89 | addSequential(new DriveDistance(POS5_DIST1, DEFAULT_SPEED)); |
90 | addSequential(new TurnForAngle(POS5_TURN1, maxTimeout)); | |
91 | addSequential(new DriveDistance(POS5_DIST2, DEFAULT_SPEED)); | |
0b403e60 ME |
92 | } |
93 | } | |
0b403e60 | 94 | } |