Commit | Line | Data |
---|---|---|
fb75626b KZ |
1 | package org.usfirst.frc.team3501.robot.commands.auton; |
2 | ||
9728080f | 3 | import org.usfirst.frc.team3501.robot.Constants; |
fb75626b | 4 | import org.usfirst.frc.team3501.robot.commands.driving.DriveDistance; |
9728080f | 5 | import org.usfirst.frc.team3501.robot.commands.driving.DriveForTime; |
fb75626b KZ |
6 | import org.usfirst.frc.team3501.robot.commands.driving.TurnForAngle; |
7 | ||
8 | import edu.wpi.first.wpilibj.command.CommandGroup; | |
9 | ||
10 | /** | |
11 | * This command group will be used in autonomous. Based on what position the | |
12 | * robot is in, the robot will align with the goal. In the Software 2015-2016 | |
13 | * Google folder is a picture explaining each of the cases. | |
14 | * | |
15 | * dependency on sensors: lidars, encoders, gyro | |
16 | * | |
17 | * dependency on subsystems: drivetrain | |
18 | * | |
19 | * dependency on other commands: TurnForAngle(), DriveForDistance() | |
20 | * | |
21 | * pre-condition: robot is flush against a defense at the specified position in | |
22 | * the opponent's courtyard | |
23 | * | |
24 | * post-condition: the robot is parallel to one of the three goals and the | |
25 | * shooter is facing that goal | |
26 | * | |
27 | */ | |
28 | public class AlignToScore extends CommandGroup { | |
fb75626b KZ |
29 | |
30 | private final double DEFAULT_SPEED = 0.5; | |
31 | private final double maxTimeout = 5; | |
32 | ||
33 | // in inches | |
34 | // assuming that positive angle means turning right | |
35 | // and negative angle means turning left | |
36 | ||
37 | // constants for position 1: low bar | |
fb75626b KZ |
38 | |
39 | public double horizontalDistToGoal; | |
40 | ||
41 | public AlignToScore(int position) { | |
ca325a58 | 42 | if (Constants.Auton.isUsingTime) { |
9728080f KZ |
43 | if (position == 1) { |
44 | addSequential(new DriveForTime(Constants.Auton.POS1_DIST1_TIME, | |
45 | Constants.Auton.POS1_DRIVE_MAXSPEED)); | |
46 | addSequential(new TurnForAngle(Constants.Auton.POS1_TURN1, | |
47 | Constants.Auton.POS1_TURN_MAXSPEED)); | |
48 | addSequential(new DriveDistance(Constants.Auton.POS1_DIST2_TIME, | |
49 | Constants.Auton.POS1_DRIVE_MAXSPEED)); | |
50 | horizontalDistToGoal = 0; | |
51 | } | |
52 | else if (position == 2) { | |
53 | addSequential(new DriveForTime(Constants.Auton.POS2_DIST1_TIME, | |
54 | Constants.Auton.POS2_DRIVE_MAXSPEED)); | |
55 | addSequential(new TurnForAngle(Constants.Auton.POS2_TURN1, | |
56 | Constants.Auton.POS2_TURN_MAXSPEED)); | |
57 | addSequential(new DriveDistance(Constants.Auton.POS2_DIST2_TIME, | |
58 | Constants.Auton.POS2_DRIVE_MAXSPEED)); | |
59 | horizontalDistToGoal = 0; | |
60 | } | |
61 | else if (position == 3) { | |
62 | addSequential(new DriveForTime(Constants.Auton.POS3_DIST1_TIME, | |
63 | Constants.Auton.POS3_DRIVE_MAXSPEED)); | |
64 | addSequential(new TurnForAngle(Constants.Auton.POS3_TURN1, | |
65 | Constants.Auton.POS3_TURN_MAXSPEED)); | |
66 | addSequential(new DriveDistance(Constants.Auton.POS3_DIST2_TIME, | |
67 | Constants.Auton.POS3_DRIVE_MAXSPEED)); | |
68 | horizontalDistToGoal = 0; | |
69 | } | |
70 | else if (position == 4) { | |
71 | addSequential(new DriveForTime(Constants.Auton.POS4_DIST1_TIME, | |
72 | Constants.Auton.POS4_DRIVE_MAXSPEED)); | |
73 | addSequential(new TurnForAngle(Constants.Auton.POS4_TURN1, | |
74 | Constants.Auton.POS4_TURN_MAXSPEED)); | |
75 | addSequential(new DriveDistance(Constants.Auton.POS4_DIST2_TIME, | |
76 | Constants.Auton.POS4_DRIVE_MAXSPEED)); | |
77 | horizontalDistToGoal = 0; | |
78 | } | |
79 | else if (position == 5) { | |
80 | addSequential(new DriveForTime(Constants.Auton.POS5_DIST1_TIME, | |
81 | Constants.Auton.POS5_DRIVE_MAXSPEED)); | |
82 | addSequential(new TurnForAngle(Constants.Auton.POS5_TURN1, | |
83 | Constants.Auton.POS5_TURN_MAXSPEED)); | |
84 | addSequential(new DriveDistance(Constants.Auton.POS5_DIST2_TIME, | |
85 | Constants.Auton.POS5_DRIVE_MAXSPEED)); | |
86 | horizontalDistToGoal = 0; | |
87 | } | |
88 | } | |
89 | else { | |
fb75626b | 90 | // position 1 is always the low bar |
9728080f KZ |
91 | if (position == 1) { |
92 | addSequential(new DriveDistance(Constants.Auton.POS1_DIST1, | |
93 | Constants.Auton.DRIVE_MAX_TIMEOUT)); | |
94 | addSequential(new TurnForAngle(Constants.Auton.POS1_TURN1, | |
95 | Constants.Auton.TURN_MAX_TIMEOUT)); | |
96 | addSequential(new DriveDistance(Constants.Auton.POS1_DIST2, | |
97 | Constants.Auton.DRIVE_MAX_TIMEOUT)); | |
98 | } else if (position == 2) { | |
99 | ||
100 | addSequential(new DriveDistance(Constants.Auton.POS2_DIST1, | |
101 | Constants.Auton.DRIVE_MAX_TIMEOUT)); | |
102 | addSequential(new TurnForAngle(Constants.Auton.POS2_TURN1, | |
103 | Constants.Auton.TURN_MAX_TIMEOUT)); | |
104 | addSequential(new DriveDistance(Constants.Auton.POS2_DIST2, | |
105 | Constants.Auton.DRIVE_MAX_TIMEOUT)); | |
106 | ||
107 | } else if (position == 3) { | |
108 | ||
109 | addSequential(new DriveDistance(Constants.Auton.POS3_DIST1, | |
110 | Constants.Auton.DRIVE_MAX_TIMEOUT)); | |
111 | addSequential(new TurnForAngle(Constants.Auton.POS3_TURN1, maxTimeout)); | |
112 | addSequential(new DriveDistance(Constants.Auton.POS3_DIST2, | |
113 | Constants.Auton.TURN_MAX_TIMEOUT)); | |
114 | addSequential(new TurnForAngle(Constants.Auton.POS3_TURN2, maxTimeout)); | |
115 | addSequential(new DriveDistance(Constants.Auton.POS3_DIST3, | |
116 | Constants.Auton.DRIVE_MAX_TIMEOUT)); | |
117 | ||
118 | } else if (position == 4) { | |
119 | ||
120 | addSequential(new DriveDistance(Constants.Auton.POS4_DIST1, | |
121 | Constants.Auton.DRIVE_MAX_TIMEOUT)); | |
122 | addSequential(new TurnForAngle(Constants.Auton.POS4_TURN1, maxTimeout)); | |
123 | addSequential(new DriveDistance(Constants.Auton.POS4_DIST2, | |
124 | Constants.Auton.TURN_MAX_TIMEOUT)); | |
125 | addSequential(new TurnForAngle(Constants.Auton.POS4_TURN2, maxTimeout)); | |
126 | addSequential(new DriveDistance(Constants.Auton.POS4_DIST3, | |
127 | Constants.Auton.DRIVE_MAX_TIMEOUT)); | |
128 | ||
129 | } else if (position == 5) { | |
130 | ||
131 | addSequential(new DriveDistance(Constants.Auton.POS5_DIST1, | |
132 | Constants.Auton.DRIVE_MAX_TIMEOUT)); | |
133 | addSequential(new TurnForAngle(Constants.Auton.POS5_TURN1, maxTimeout)); | |
134 | addSequential(new DriveDistance(Constants.Auton.POS5_DIST2, | |
135 | Constants.Auton.TURN_MAX_TIMEOUT)); | |
136 | addSequential(new TurnForAngle(Constants.Auton.POS5_TURN2, maxTimeout)); | |
137 | addSequential(new DriveDistance(Constants.Auton.POS5_DIST3, | |
138 | Constants.Auton.DRIVE_MAX_TIMEOUT)); | |
139 | } | |
fb75626b KZ |
140 | } |
141 | } | |
fb75626b KZ |
142 | // following commented out method is calculations for path of robot in auton |
143 | // after passing through defense using two lidars | |
144 | /* | |
145 | * public static double lidarCalculateAngleToTurn(int position, | |
146 | * double horizontalDistToGoal) { | |
147 | * double leftDist = Robot.driveTrain.getLeftLidarDistance(); | |
148 | * double rightDist = Robot.driveTrain.getRightLidarDistance(); | |
600a1a1c | 149 | * |
fb75626b KZ |
150 | * double errorAngle = Math.atan(Math.abs(leftDist - rightDist) / 2); |
151 | * double distToTower; | |
152 | * // TODO: figure out if we do want to shoot into the side goal if we are | |
153 | * // in position 1 or 2, or if we want to change that | |
154 | * if (position == 1 || position == 2) { | |
155 | * distToTower = Math | |
156 | * .cos(CENTER_OF_MASS_TO_ROBOT_FRONT + (leftDist - rightDist) / 2) | |
157 | * - DIST_CASTLE_WALL_TO_SIDE_GOAL; | |
158 | * } | |
600a1a1c | 159 | * |
fb75626b KZ |
160 | * // TODO: figure out if we do want to shoot into the font goal if we are |
161 | * // in position 3, 4, 5, or if we want to change that | |
162 | * else { | |
163 | * distToTower = Math | |
164 | * .cos(CENTER_OF_MASS_TO_ROBOT_FRONT + (leftDist - rightDist) / 2) | |
165 | * - DIST_CASTLE_WALL_TO_SIDE_GOAL; | |
166 | * } | |
600a1a1c | 167 | * |
fb75626b | 168 | * double angleToTurn = Math.atan(distToTower / horizontalDistToGoal); |
600a1a1c | 169 | * |
fb75626b KZ |
170 | * return angleToTurn; |
171 | * } | |
172 | */ | |
173 | } |