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