blob: 425f6d3276d7c34a49df4305ec8471eb6e4e8163 [file] [log] [blame]
Austin Schuha3c148e2018-03-09 21:04:05 -08001#include "y2018/actors/autonomous_actor.h"
2
3#include <inttypes.h>
4
5#include <chrono>
6#include <cmath>
7
John Park33858a32018-09-28 23:05:48 -07008#include "aos/logging/logging.h"
James Kuszmaul651fc3f2019-05-15 21:14:25 -07009#include "aos/util/phased_loop.h"
Austin Schuha3c148e2018-03-09 21:04:05 -080010
11#include "frc971/control_loops/drivetrain/drivetrain.q.h"
12#include "y2018/control_loops/drivetrain/drivetrain_base.h"
13
14namespace y2018 {
15namespace actors {
16using ::frc971::ProfileParameters;
17
Austin Schuha3c148e2018-03-09 21:04:05 -080018using ::aos::monotonic_clock;
James Kuszmaul651fc3f2019-05-15 21:14:25 -070019using ::frc971::control_loops::drivetrain_queue;
Austin Schuha3c148e2018-03-09 21:04:05 -080020namespace chrono = ::std::chrono;
21namespace this_thread = ::std::this_thread;
22
23namespace {
24
Austin Schuh4dad8fb2018-07-08 16:00:13 -070025const ProfileParameters kFinalSwitchDrive = {0.5, 1.5};
Austin Schuhc231df42018-03-21 20:43:24 -070026const ProfileParameters kDrive = {5.0, 2.5};
Austin Schuhf79c0e52018-04-04 20:13:21 -070027const ProfileParameters kThirdBoxDrive = {3.0, 2.5};
Austin Schuhc231df42018-03-21 20:43:24 -070028const ProfileParameters kSlowDrive = {1.5, 2.5};
Austin Schuhf79c0e52018-04-04 20:13:21 -070029const ProfileParameters kScaleTurnDrive = {3.0, 2.5};
Austin Schuhc231df42018-03-21 20:43:24 -070030const ProfileParameters kFarSwitchTurnDrive = {2.0, 2.5};
Austin Schuh4dad8fb2018-07-08 16:00:13 -070031const ProfileParameters kFarScaleFinalTurnDrive = kFarSwitchTurnDrive;
Austin Schuhc231df42018-03-21 20:43:24 -070032const ProfileParameters kTurn = {4.0, 2.0};
33const ProfileParameters kSweepingTurn = {5.0, 7.0};
Austin Schuh4dad8fb2018-07-08 16:00:13 -070034const ProfileParameters kFarScaleSweepingTurn = kSweepingTurn;
Austin Schuhc231df42018-03-21 20:43:24 -070035const ProfileParameters kFastTurn = {5.0, 7.0};
Austin Schuhf79c0e52018-04-04 20:13:21 -070036const ProfileParameters kReallyFastTurn = {1.5, 9.0};
37
38const ProfileParameters kThirdBoxSlowBackup = {0.35, 1.5};
39const ProfileParameters kThirdBoxSlowTurn = {1.5, 4.0};
40
Austin Schuh4dad8fb2018-07-08 16:00:13 -070041const ProfileParameters kThirdBoxPlaceDrive = {4.0, 2.0};
Austin Schuhf79c0e52018-04-04 20:13:21 -070042
43const ProfileParameters kFinalFrontFarSwitchDrive = {2.0, 2.0};
Austin Schuha3c148e2018-03-09 21:04:05 -080044
45} // namespace
46
Austin Schuh1bf8a212019-05-26 22:13:14 -070047AutonomousActor::AutonomousActor(::aos::EventLoop *event_loop)
Austin Schuha3c148e2018-03-09 21:04:05 -080048 : frc971::autonomous::BaseAutonomousActor(
Austin Schuh1bf8a212019-05-26 22:13:14 -070049 event_loop, control_loops::drivetrain::GetDrivetrainConfig()) {}
Austin Schuha3c148e2018-03-09 21:04:05 -080050
51bool AutonomousActor::RunAction(
52 const ::frc971::autonomous::AutonomousActionParams &params) {
53 monotonic_clock::time_point start_time = monotonic_clock::now();
54 LOG(INFO, "Starting autonomous action with mode %" PRId32 "\n", params.mode);
55 Reset();
56
Austin Schuh4dad8fb2018-07-08 16:00:13 -070057 // Switch
58 /*
Austin Schuhc231df42018-03-21 20:43:24 -070059 switch (params.mode) {
Austin Schuh4dad8fb2018-07-08 16:00:13 -070060 case 0:
61 case 2: {
62 if (FarSwitch(start_time, true)) return true;
63 } break;
64
65 case 3:
Austin Schuhc231df42018-03-21 20:43:24 -070066 case 1: {
Austin Schuh4dad8fb2018-07-08 16:00:13 -070067 if (CloseSwitch(start_time)) return true;
Austin Schuhc231df42018-03-21 20:43:24 -070068 } break;
Austin Schuh4dad8fb2018-07-08 16:00:13 -070069 }
70 */
71 // Scale
72 switch (params.mode) {
73 case 0:
74 case 1: {
75 if (FarScale(start_time)) return true;
Austin Schuhc231df42018-03-21 20:43:24 -070076 } break;
77
78 case 3:
79 case 2: {
Austin Schuh4dad8fb2018-07-08 16:00:13 -070080 if (ThreeCubeAuto(start_time)) return true;
Austin Schuhc231df42018-03-21 20:43:24 -070081 } break;
Austin Schuha3c148e2018-03-09 21:04:05 -080082 }
Austin Schuh4dad8fb2018-07-08 16:00:13 -070083 /*
84 switch (params.mode) {
85 case 1: {
86 if (FarScale(start_time)) return true;
87 //if (CloseSwitch(start_time)) return true;
88 } break;
89 case 0: {
90 if (DriveStraight()) return true;
91 } break;
92 case 200: {
93 if (FarSwitch(start_time)) return true;
94 } break;
95
96 case 3:
97 case 2: {
98 if (ThreeCubeAuto(start_time)) {
99 return true;
100 }
101 } break;
102 }
103 */
Austin Schuha3c148e2018-03-09 21:04:05 -0800104
James Kuszmaul651fc3f2019-05-15 21:14:25 -0700105 LOG(INFO, "Done %f\n",
106 ::aos::time::DurationInSeconds(monotonic_clock::now() - start_time));
Austin Schuha3c148e2018-03-09 21:04:05 -0800107
108 ::aos::time::PhasedLoop phased_loop(::std::chrono::milliseconds(5),
109 ::std::chrono::milliseconds(5) / 2);
110
111 while (!ShouldCancel()) {
112 phased_loop.SleepUntilNext();
113 }
114 LOG(DEBUG, "Done running\n");
115
116 return true;
117}
118
Austin Schuh4dad8fb2018-07-08 16:00:13 -0700119bool AutonomousActor::FarSwitch(monotonic_clock::time_point start_time,
120 bool drive_behind, bool left) {
121 const double turn_scalar = left ? 1.0 : -1.0;
122 if (drive_behind) {
123 // Start on the left. Hit the switch.
124 constexpr double kFullDriveLength = 9.93;
125 constexpr double kTurnDistance = 4.40;
126 StartDrive(-kFullDriveLength, 0.0, kDrive, kTurn);
127 set_arm_goal_position(arm::NeutralIndex());
128 set_grab_box(false);
129 SendSuperstructureGoal();
130 if (!WaitForDriveProfileNear(kFullDriveLength - (kTurnDistance - 1.4)))
131 return true;
132 StartDrive(0.0, 0.0, kFarSwitchTurnDrive, kTurn);
133
134 if (!WaitForDriveProfileNear(kFullDriveLength - kTurnDistance)) return true;
James Kuszmaul651fc3f2019-05-15 21:14:25 -0700135 StartDrive(0.0, turn_scalar * (-M_PI / 2.0), kFarSwitchTurnDrive,
136 kSweepingTurn);
Austin Schuh4dad8fb2018-07-08 16:00:13 -0700137 if (!WaitForTurnProfileDone()) return true;
138
139 // Now, close so let's move the arm up.
140 set_arm_goal_position(arm::FrontSwitchAutoIndex());
141 SendSuperstructureGoal();
142
143 StartDrive(0.0, 0.0, kDrive, kTurn);
144 if (!WaitForDriveProfileNear(2.0)) return true;
145 StartDrive(0.0, 0.0, kFarSwitchTurnDrive, kFastTurn);
146
147 if (!WaitForDriveProfileNear(1.3)) return true;
148
149 StartDrive(0.0, turn_scalar * M_PI / 2.0, kFarSwitchTurnDrive, kFastTurn);
150 if (!WaitForTurnProfileDone()) return true;
151
152 constexpr double kGentlePushDrive = 0.70;
153 StartDrive(kGentlePushDrive, 0.0, kSlowDrive, kTurn);
154
155 if (!WaitForDriveProfileNear(kGentlePushDrive - 0.25)) return true;
156 // Turn down the peak voltage when we push against the wall so we don't blow
157 // breakers or pull the battery voltage down too far.
158 set_max_drivetrain_voltage(6.0);
159 StartDrive(0.00, 0.0, kFinalSwitchDrive, kTurn);
160
161 if (!WaitForArmTrajectoryClose(0.001)) return true;
162 LOG(INFO, "Arm close at %f\n",
James Kuszmaul651fc3f2019-05-15 21:14:25 -0700163 ::aos::time::DurationInSeconds(monotonic_clock::now() - start_time));
Austin Schuh4dad8fb2018-07-08 16:00:13 -0700164
165 ::std::this_thread::sleep_for(chrono::milliseconds(1000));
166
167 set_open_claw(true);
168 SendSuperstructureGoal();
169
170 ::std::this_thread::sleep_for(chrono::milliseconds(1500));
171 set_arm_goal_position(arm::NeutralIndex());
172 SendSuperstructureGoal();
173 if (ShouldCancel()) return true;
174 set_max_drivetrain_voltage(12.0);
175 StartDrive(-0.5, 0.0, kDrive, kTurn);
176 if (!WaitForDriveProfileDone()) return true;
177 } else {
178 // Start on the left. Hit the switch.
179 constexpr double kFullDriveLength = 5.55;
180 constexpr double kTurnDistance = 0.35;
181 StartDrive(-kFullDriveLength, 0.0, kFarSwitchTurnDrive, kTurn);
182
183 if (!WaitForDriveProfileNear(kFullDriveLength - kTurnDistance)) return true;
184 StartDrive(0.0, turn_scalar * (-M_PI / 2.0), kFarSwitchTurnDrive,
185 kSweepingTurn);
186 if (!WaitForTurnProfileDone()) return true;
187 StartDrive(0.0, 0.0, kDrive, kTurn);
188 if (!WaitForDriveProfileDone()) return true;
189
190 // Now, close so let's move the arm up.
191 set_arm_goal_position(arm::FrontSwitchIndex());
192 SendSuperstructureGoal();
193
194 StartDrive(0.0, turn_scalar * (-M_PI / 2.0), kDrive, kFastTurn);
195 if (!WaitForTurnProfileDone()) return true;
196
197 set_max_drivetrain_voltage(10.0);
198 StartDrive(1.1, 0.0, kDrive, kTurn);
199 if (!WaitForArmTrajectoryClose(0.001)) return true;
200 if (!WaitForDriveNear(0.6, M_PI / 2.0)) return true;
201 StartDrive(0.0, 0.0, kFinalFrontFarSwitchDrive, kTurn);
202
203 if (!WaitForDriveNear(0.3, M_PI / 2.0)) return true;
204 set_max_drivetrain_voltage(6.0);
205 StartDrive(0.0, 0.0, kFinalFrontFarSwitchDrive, kTurn);
206
207 // if (!WaitForDriveNear(0.2, M_PI / 2.0)) return true;
208 ::std::this_thread::sleep_for(chrono::milliseconds(300));
209
210 set_open_claw(true);
211 SendSuperstructureGoal();
212
213 ::std::this_thread::sleep_for(chrono::milliseconds(1000));
214 set_arm_goal_position(arm::NeutralIndex());
215 SendSuperstructureGoal();
216 if (ShouldCancel()) return true;
217 set_max_drivetrain_voltage(12.0);
218 StartDrive(-0.5, 0.0, kDrive, kTurn);
219 if (!WaitForDriveProfileDone()) return true;
220 }
221 return false;
222}
223
224bool AutonomousActor::FarScale(monotonic_clock::time_point start_time) {
225 // Start on the left. Hit the switch.
226 constexpr double kFullDriveLength = 11.40;
227 constexpr double kFirstTurnDistance = 4.40;
228 constexpr double kSecondTurnDistance = 9.30;
229 StartDrive(-kFullDriveLength, 0.0, kDrive, kTurn);
230 if (!WaitForDriveProfileNear(kFullDriveLength - (kFirstTurnDistance - 1.4)))
231 return true;
232 StartDrive(0.0, 0.0, kFarSwitchTurnDrive, kTurn);
233
James Kuszmaul651fc3f2019-05-15 21:14:25 -0700234 if (!WaitForDriveProfileNear(kFullDriveLength - kFirstTurnDistance))
235 return true;
Austin Schuh4dad8fb2018-07-08 16:00:13 -0700236 StartDrive(0.0, -M_PI / 2.0, kFarSwitchTurnDrive, kSweepingTurn);
237 set_arm_goal_position(arm::BackHighBoxIndex());
238 SendSuperstructureGoal();
239 if (!WaitForTurnProfileDone()) return true;
240
241 StartDrive(0.0, 0.0, kDrive, kTurn);
242
243 if (!WaitForDriveProfileNear(kFullDriveLength - (kSecondTurnDistance - 1.4)))
244 return true;
245
246 StartDrive(0.0, 0.0, kFarScaleFinalTurnDrive, kTurn);
247 if (!WaitForDriveProfileNear(kFullDriveLength - (kSecondTurnDistance)))
248 return true;
249 LOG(INFO, "Final turn at %f\n",
James Kuszmaul651fc3f2019-05-15 21:14:25 -0700250 ::aos::time::DurationInSeconds(monotonic_clock::now() - start_time));
Austin Schuh4dad8fb2018-07-08 16:00:13 -0700251
252 StartDrive(0.0, M_PI / 2.0, kFarScaleFinalTurnDrive, kFarScaleSweepingTurn);
253 if (!WaitForDriveProfileNear(0.15)) return true;
254
255 StartDrive(0.0, 0.3, kFarScaleFinalTurnDrive, kFarScaleSweepingTurn);
256
257 LOG(INFO, "Dropping at %f\n",
James Kuszmaul651fc3f2019-05-15 21:14:25 -0700258 ::aos::time::DurationInSeconds(monotonic_clock::now() - start_time));
Austin Schuh4dad8fb2018-07-08 16:00:13 -0700259 set_open_claw(true);
260 SendSuperstructureGoal();
261
262 ::std::this_thread::sleep_for(chrono::milliseconds(1000));
263 LOG(INFO, "Backing up at %f\n",
James Kuszmaul651fc3f2019-05-15 21:14:25 -0700264 ::aos::time::DurationInSeconds(monotonic_clock::now() - start_time));
Austin Schuh4dad8fb2018-07-08 16:00:13 -0700265
266 StartDrive(1.5, -0.55, kFarScaleFinalTurnDrive, kFarScaleSweepingTurn);
267
268 if (!WaitForDriveProfileNear(1.4)) return true;
269 set_arm_goal_position(arm::NeutralIndex());
270 set_open_claw(false);
271 set_grab_box(true);
272 SendSuperstructureGoal();
273
274 if (!WaitForDriveProfileNear(0.3)) return true;
275
276 set_intake_angle(0.15);
277 set_roller_voltage(10.0);
278 SendSuperstructureGoal();
279
280 set_max_drivetrain_voltage(6.0);
281 StartDrive(0.0, 0.0, kFarScaleFinalTurnDrive, kFarScaleSweepingTurn);
282
283 if (!WaitForDriveProfileDone()) return true;
284 if (!WaitForTurnProfileDone()) return true;
285
286 StartDrive(0.2, 0.0, kThirdBoxSlowBackup, kThirdBoxSlowTurn);
287
288 if (!WaitForBoxGrabed()) return true;
289 set_arm_goal_position(arm::BackHighBoxIndex());
290 set_intake_angle(-3.0);
291 set_roller_voltage(0.0);
292 set_grab_box(false);
293 SendSuperstructureGoal();
294
295 StartDrive(-1.40, 0.15, kThirdBoxPlaceDrive, kFarScaleSweepingTurn);
296
297 if (!WaitForDriveProfileNear(0.1)) return true;
298
299 StartDrive(0.0, 0.5, kThirdBoxPlaceDrive, kFarScaleSweepingTurn);
300
301 if (!WaitForDriveProfileDone()) return true;
302 if (!WaitForTurnProfileDone()) return true;
303 ::std::this_thread::sleep_for(chrono::milliseconds(500));
304
305 LOG(INFO, "Dropping second box at %f\n",
James Kuszmaul651fc3f2019-05-15 21:14:25 -0700306 ::aos::time::DurationInSeconds(monotonic_clock::now() - start_time));
Austin Schuh4dad8fb2018-07-08 16:00:13 -0700307 set_open_claw(true);
308 SendSuperstructureGoal();
309
310 ::std::this_thread::sleep_for(chrono::milliseconds(1000));
311
312 StartDrive(1.4, -0.7, kThirdBoxPlaceDrive, kFarScaleSweepingTurn);
313 ::std::this_thread::sleep_for(chrono::milliseconds(200));
314 set_arm_goal_position(arm::NeutralIndex());
315 set_open_claw(false);
316 SendSuperstructureGoal();
317
318 if (!WaitForDriveProfileNear(1.0)) return true;
319
320 StartDrive(0.0, -0.6, kThirdBoxPlaceDrive, kFarScaleSweepingTurn);
321 return false;
322}
323
324bool AutonomousActor::FarReadyScale(monotonic_clock::time_point start_time) {
325 // Start on the left. Hit the switch.
326 constexpr double kFullDriveLength = 7.5;
327 constexpr double kFirstTurnDistance = 4.40;
328 StartDrive(-kFullDriveLength, 0.0, kDrive, kTurn);
329 if (!WaitForDriveProfileNear(kFullDriveLength - (kFirstTurnDistance - 1.4)))
330 return true;
331 StartDrive(0.0, 0.0, kFarSwitchTurnDrive, kTurn);
332
James Kuszmaul651fc3f2019-05-15 21:14:25 -0700333 if (!WaitForDriveProfileNear(kFullDriveLength - kFirstTurnDistance))
334 return true;
Austin Schuh4dad8fb2018-07-08 16:00:13 -0700335 StartDrive(0.0, -M_PI / 2.0, kFarSwitchTurnDrive, kSweepingTurn);
336 set_arm_goal_position(arm::UpIndex());
337 SendSuperstructureGoal();
338 LOG(INFO, "Lifting arm at %f\n",
James Kuszmaul651fc3f2019-05-15 21:14:25 -0700339 ::aos::time::DurationInSeconds(monotonic_clock::now() - start_time));
Austin Schuh4dad8fb2018-07-08 16:00:13 -0700340 if (!WaitForTurnProfileDone()) return true;
341
342 StartDrive(0.0, 0.0, kDrive, kTurn);
343 return false;
344}
345
346bool AutonomousActor::DriveStraight() {
347 StartDrive(-3.2, 0.0, kDrive, kTurn);
348 if (!WaitForDriveProfileDone()) return true;
349 return false;
350}
351
352bool AutonomousActor::CloseSwitch(monotonic_clock::time_point start_time,
353 bool left) {
354 const double turn_scalar = left ? 1.0 : -1.0;
355
356 constexpr double kDriveDistance = 3.2;
357 // Start on the left. Drive, arc a turn, and drop in the close switch.
358 StartDrive(-kDriveDistance, 0.0, kDrive, kTurn);
359 if (!WaitForDriveNear(2.5, M_PI / 2.0)) return true;
360
361 // Now, close so let's move the arm up.
362 set_arm_goal_position(arm::BackSwitchIndex());
363 SendSuperstructureGoal();
364
365 StartDrive(0.0, 0.0, kSlowDrive, kSweepingTurn);
366 if (!WaitForDriveNear(1.6, M_PI / 2.0)) return true;
367
368 StartDrive(0.0, turn_scalar * (-M_PI / 4.0 - 0.2), kSlowDrive, kSweepingTurn);
369 if (!WaitForDriveNear(0.2, 0.2)) return true;
370 set_max_drivetrain_voltage(6.0);
371 LOG(INFO, "Lowered drivetrain voltage %f\n",
James Kuszmaul651fc3f2019-05-15 21:14:25 -0700372 ::aos::time::DurationInSeconds(monotonic_clock::now() - start_time));
Austin Schuh4dad8fb2018-07-08 16:00:13 -0700373 ::std::this_thread::sleep_for(chrono::milliseconds(300));
374
375 set_open_claw(true);
376 SendSuperstructureGoal();
377
378 ::std::this_thread::sleep_for(chrono::milliseconds(500));
379 set_max_drivetrain_voltage(12.0);
380 StartDrive(0.7, 0.0, kDrive, kTurn);
381 if (!WaitForTurnProfileDone()) return true;
382
383 set_arm_goal_position(arm::NeutralIndex());
384 SendSuperstructureGoal();
385 return false;
386}
387
388bool AutonomousActor::ThreeCubeAuto(monotonic_clock::time_point start_time) {
389 // Start on the left. Hit the scale.
390 constexpr double kDriveDistance = 6.95;
391 // Distance and angle to do the big drive to the third cube.
392 constexpr double kThirdCubeDrive = 1.57 + 0.05 + 0.05;
393 constexpr double kThirdCubeTurn = M_PI / 4.0 + 0.1;
394 // Angle to do the slow pickup turn on the third box.
395 constexpr double kThirdBoxEndTurnAngle = 0.30;
396
397 // Distance to drive back to the scale with the third cube.
398 constexpr double kThirdCubeDropDrive = kThirdCubeDrive + 0.40;
399
400 // Drive.
401 StartDrive(-kDriveDistance, 0.0, kDrive, kTurn);
402 if (!WaitForDriveNear(kDriveDistance - 1.0, M_PI / 2.0)) return true;
403 // Once we are away from the wall, start the arm.
404 set_arm_goal_position(arm::BackMiddle2BoxIndex());
405 SendSuperstructureGoal();
406
407 // We are starting to get close. Slow down for the turn.
408 if (!WaitForDriveNear(4.0, M_PI / 2.0)) return true;
409 StartDrive(0.0, 0.0, kScaleTurnDrive, kFastTurn);
410
411 // Once we've gotten slowed down a bit, start turning.
412 if (!WaitForDriveNear(3.25, M_PI / 2.0)) return true;
413 StartDrive(0.0, -M_PI / 6.0, kScaleTurnDrive, kFastTurn);
414
415 if (!WaitForDriveNear(1.0, M_PI / 2.0)) return true;
416 StartDrive(0.0, M_PI / 6.0, kScaleTurnDrive, kFastTurn);
417
418 // Get close and open the claw.
419 if (!WaitForDriveNear(0.15, 0.25)) return true;
420 set_open_claw(true);
421 SendSuperstructureGoal();
422 set_intake_angle(-0.60);
423 LOG(INFO, "Dropped first box %f\n",
James Kuszmaul651fc3f2019-05-15 21:14:25 -0700424 ::aos::time::DurationInSeconds(monotonic_clock::now() - start_time));
Austin Schuh4dad8fb2018-07-08 16:00:13 -0700425
426 ::std::this_thread::sleep_for(chrono::milliseconds(700));
427
428 set_grab_box(true);
429 SendSuperstructureGoal();
430
431 LOG(INFO, "Starting second box drive %f\n",
James Kuszmaul651fc3f2019-05-15 21:14:25 -0700432 ::aos::time::DurationInSeconds(monotonic_clock::now() - start_time));
Austin Schuh4dad8fb2018-07-08 16:00:13 -0700433 constexpr double kSecondBoxSwerveAngle = 0.35;
434 constexpr double kSecondBoxDrive = 1.38;
435 StartDrive(kSecondBoxDrive, 0.0, kDrive, kFastTurn);
436 if (!WaitForDriveNear(kSecondBoxDrive - 0.2, M_PI / 2.0)) return true;
437
438 StartDrive(0.0, kSecondBoxSwerveAngle, kDrive, kFastTurn);
439 if (!WaitForDriveNear(0.5, M_PI / 2.0)) return true;
440
441 set_open_claw(true);
442 set_disable_box_correct(false);
443 SendSuperstructureGoal();
444
445 StartDrive(0.0, -kSecondBoxSwerveAngle, kDrive, kFastTurn);
446
447 if (!WaitForDriveProfileDone()) return true;
448 set_max_drivetrain_voltage(6.0);
449 StartDrive(0.0, 0.0, kDrive, kFastTurn);
450
451 set_intake_angle(0.15);
452 set_arm_goal_position(arm::BackHighBoxIndex());
453 set_open_claw(false);
454
455 set_roller_voltage(10.0);
456 SendSuperstructureGoal();
457
458 LOG(INFO, "Grabbing second box %f\n",
James Kuszmaul651fc3f2019-05-15 21:14:25 -0700459 ::aos::time::DurationInSeconds(monotonic_clock::now() - start_time));
Austin Schuh4dad8fb2018-07-08 16:00:13 -0700460 ::std::this_thread::sleep_for(chrono::milliseconds(200));
461 StartDrive(-0.04, 0.0, kThirdBoxSlowBackup, kThirdBoxSlowTurn);
462
463 if (!WaitForBoxGrabed()) return true;
464 set_max_drivetrain_voltage(12.0);
465
466 LOG(INFO, "Got second box %f\n",
James Kuszmaul651fc3f2019-05-15 21:14:25 -0700467 ::aos::time::DurationInSeconds(monotonic_clock::now() - start_time));
Austin Schuh4dad8fb2018-07-08 16:00:13 -0700468 ::std::this_thread::sleep_for(chrono::milliseconds(500));
469
470 set_grab_box(false);
James Kuszmaul651fc3f2019-05-15 21:14:25 -0700471 // set_arm_goal_position(arm::UpIndex());
Austin Schuh4dad8fb2018-07-08 16:00:13 -0700472 set_arm_goal_position(arm::BackHighBoxIndex());
473 set_roller_voltage(0.0);
474 set_disable_box_correct(false);
475 SendSuperstructureGoal();
476 LOG(INFO, "Driving to place second box %f\n",
James Kuszmaul651fc3f2019-05-15 21:14:25 -0700477 ::aos::time::DurationInSeconds(monotonic_clock::now() - start_time));
Austin Schuh4dad8fb2018-07-08 16:00:13 -0700478
479 StartDrive(-kSecondBoxDrive + 0.16, kSecondBoxSwerveAngle, kDrive, kFastTurn);
480 if (!WaitForDriveNear(0.4, M_PI / 2.0)) return true;
481
482 constexpr double kSecondBoxEndExtraAngle = 0.3;
483 StartDrive(0.0, -kSecondBoxSwerveAngle - kSecondBoxEndExtraAngle, kDrive,
484 kFastTurn);
485
486 LOG(INFO, "Starting throw %f\n",
James Kuszmaul651fc3f2019-05-15 21:14:25 -0700487 ::aos::time::DurationInSeconds(monotonic_clock::now() - start_time));
Austin Schuh4dad8fb2018-07-08 16:00:13 -0700488 if (!WaitForDriveNear(0.4, M_PI / 2.0)) return true;
489 if (!WaitForArmTrajectoryClose(0.25)) return true;
490 SendSuperstructureGoal();
491
492 // Throw the box.
James Kuszmaul651fc3f2019-05-15 21:14:25 -0700493 // if (!WaitForArmTrajectoryClose(0.20)) return true;
Austin Schuh4dad8fb2018-07-08 16:00:13 -0700494 if (!WaitForDriveNear(0.2, M_PI / 2.0)) return true;
495
496 set_open_claw(true);
497 set_intake_angle(-M_PI / 4.0);
498 LOG(INFO, "Releasing second box %f\n",
James Kuszmaul651fc3f2019-05-15 21:14:25 -0700499 ::aos::time::DurationInSeconds(monotonic_clock::now() - start_time));
Austin Schuh4dad8fb2018-07-08 16:00:13 -0700500 SendSuperstructureGoal();
501
502 ::std::this_thread::sleep_for(chrono::milliseconds(700));
503 set_open_claw(false);
504 SendSuperstructureGoal();
505
506 LOG(INFO, "Driving to third box %f\n",
James Kuszmaul651fc3f2019-05-15 21:14:25 -0700507 ::aos::time::DurationInSeconds(monotonic_clock::now() - start_time));
Austin Schuh4dad8fb2018-07-08 16:00:13 -0700508 StartDrive(kThirdCubeDrive, kSecondBoxEndExtraAngle, kThirdBoxDrive,
509 kFastTurn);
510 if (!WaitForDriveNear(kThirdCubeDrive - 0.1, M_PI / 4.0)) return true;
511 set_grab_box(true);
512 SendSuperstructureGoal();
513
514 StartDrive(0.0, kThirdCubeTurn, kThirdBoxDrive, kFastTurn);
515 if (!WaitForDriveNear(0.05, M_PI / 4.0)) return true;
516
517 set_intake_angle(0.05);
518 set_roller_voltage(9.0);
519 SendSuperstructureGoal();
520
521 if (!WaitForDriveProfileDone()) return true;
522 if (!WaitForTurnProfileDone()) return true;
523 StartDrive(0.35, kThirdBoxEndTurnAngle, kThirdBoxSlowBackup,
524 kThirdBoxSlowTurn);
525 if (!WaitForDriveProfileDone()) return true;
526
527 LOG(INFO, "Waiting for third box %f\n",
James Kuszmaul651fc3f2019-05-15 21:14:25 -0700528 ::aos::time::DurationInSeconds(monotonic_clock::now() - start_time));
Austin Schuh4dad8fb2018-07-08 16:00:13 -0700529 if (!WaitForBoxGrabed()) return true;
530 LOG(INFO, "Third box grabbed %f\n",
James Kuszmaul651fc3f2019-05-15 21:14:25 -0700531 ::aos::time::DurationInSeconds(monotonic_clock::now() - start_time));
Austin Schuh4dad8fb2018-07-08 16:00:13 -0700532 const bool too_late =
533 monotonic_clock::now() > start_time + chrono::milliseconds(12500);
534 if (too_late) {
535 LOG(INFO, "Third box too long, going up. %f\n",
James Kuszmaul651fc3f2019-05-15 21:14:25 -0700536 ::aos::time::DurationInSeconds(monotonic_clock::now() - start_time));
Austin Schuh4dad8fb2018-07-08 16:00:13 -0700537 set_grab_box(false);
538 set_arm_goal_position(arm::UpIndex());
539 set_roller_voltage(0.0);
540 SendSuperstructureGoal();
541 }
542 ::std::this_thread::sleep_for(chrono::milliseconds(400));
543
544 set_grab_box(false);
545 if (!too_late) {
546 set_arm_goal_position(arm::BackMiddle2BoxIndex());
547 }
548 set_roller_voltage(0.0);
549 SendSuperstructureGoal();
550
551 StartDrive(-kThirdCubeDropDrive, 0.0, kThirdBoxPlaceDrive, kReallyFastTurn);
552
553 if (!WaitForDriveNear(kThirdCubeDropDrive - 0.23, M_PI / 4.0)) return true;
554 StartDrive(0.0, -kThirdCubeTurn - 0.2 - kThirdBoxEndTurnAngle - 0.3,
555 kThirdBoxPlaceDrive, kReallyFastTurn);
556
557 if (!WaitForDriveNear(0.30, M_PI / 4.0 + 0.2)) return true;
558
559 if (!too_late) {
560 set_open_claw(true);
561 set_intake_angle(-M_PI / 4.0);
562 set_roller_voltage(0.0);
563 SendSuperstructureGoal();
564
565 LOG(INFO, "Final open %f\n",
James Kuszmaul651fc3f2019-05-15 21:14:25 -0700566 ::aos::time::DurationInSeconds(monotonic_clock::now() - start_time));
Austin Schuh4dad8fb2018-07-08 16:00:13 -0700567 }
568
569 if (!WaitForDriveProfileDone()) return true;
570 if (!WaitForTurnProfileDone()) return true;
571
572 ::std::this_thread::sleep_for(chrono::milliseconds(14750) -
573 (monotonic_clock::now() - start_time));
574
575 set_arm_goal_position(arm::UpIndex());
576 SendSuperstructureGoal();
577
578 ::std::this_thread::sleep_for(chrono::milliseconds(15000) -
579 (monotonic_clock::now() - start_time));
580
581 set_close_claw(true);
582 SendSuperstructureGoal();
583 return false;
584}
585
Austin Schuha3c148e2018-03-09 21:04:05 -0800586} // namespace actors
587} // namespace y2018