1516X Push Back 1.0
1516X's robot code for the 2025-2026 VEX Robotics Competition
Loading...
Searching...
No Matches
autonRoutines.cpp
Go to the documentation of this file.
1// Copyright 2026 California High Robotics, Team 1516X
2// SPDX-License-Identifier: GPL-3.0-or-later
3
4#include "IntakeAntiJam.h"
5#include "globals.h"
6#include "auton/autonFunctions.h"
7#include "lemlib/chassis/chassis.hpp"
8#include "ltv.h"
9#include "pros/rtos.hpp"
10#include "velocityController.h"
11#include "paths.h"
12#include "distanceReset.h"
13#include "colorSort.h"
14#include "crossBarrierDetection.h"
15#include <cstdlib>
16#include <map>
17#include <sys/types.h>
18#include "RCL.h"
19
20/*
21
22Straight:
23KV = 5.83366315659
241.14915679465
250.277890461166
2611.8794542097
2759.3640060487
28
29Turn:
306.69921571762
311.01377617649
320.956265367397
3314.8634454339
34101.977995601
35
36*/
37
38const int longgoal_delay = 1100;
39const int midgoal_delay = 1000;
40const int matchload_delay = 500;
41const int mid_triball_delay = 500;
42const int longgoal_offset = 6;
43const int midgoal_offset = 11.7;
44const int matchload_offset = 11.9;
45const int triball_delay = 500;
46const int dual_ball_delay = 500;
47
48const VelocityControllerConfig config{
495.09498070723,
500.23396112183,
511.22496514644,
520.964796393603,
530.522291806997,
5410.933332351,
5544.6788428062,
56};
57
58LTVPathFollower ltv(config);
59
60
61
62void precompute_auton_paths(std::string path_name) {
63 std::map<std::string, std::vector<std::string>> pathMap =
64 {
65 {"Skills", {"skills_1", "skills_2", "skills_3", "skills_4", "parkingzone_curve"}},
66 {"Right 7 Split", {"awp_1"}},
67 {"Right 7", {"right_7_1"}},
68 {"AWP", {"awp_2"}},
69 };
70 for (const auto& [key, value] : pathMap) {
71 {
72 if(path_name == key)
73 {
74 ltv.precompute_paths(pathMap[key]);
75 return;
76 }
77 }
78}
79
80
81}
82
83void test_auton()
84{
85 chassis.setPose(62.5, -33, 180);
86 chassis.setPose(62.5, -33, chassis.getPose().theta);
87 distanceReset(true, false);
88 rcl.start();
89 chassis.turnToHeading(240, 2000, {}, false);
90 distanceReset(true, false);
91 rcl.start();
92 chassis.moveToPoint(45, -47, 2000, {});
93 chassis.turnToHeading(90, 2000);
94 chassis.moveToPose(27, -47, 270, 2000, {}, false);
95 score_longgoal_auton(600, allianceColor, 1000);
96 intake();
97 chassis.moveToPose(56.5, 47, 90, 2000, {.minSpeed = 30}, false);
98 chassis.tank(0.35, 0, config, matchload_delay);
99 distanceReset(true);
100
101 rcl.start();
102 chassis.moveToPoint(50, chassis.getPose().y, 1500, {.forwards = false, .minSpeed = 80, .earlyExitRange = 8});
103 intake_stop();
104 intakeMotor.move_velocity(200);
105 chassis.moveToPose(24,-61, 90, 1500, {.forwards = false, .minSpeed = 30, .earlyExitRange = 7});
106 intake_stop();
107 matchload_state(false);
108 chassis.moveToPoint(-29, -58.5, 1500, {.forwards = false, .minSpeed = 80, .earlyExitRange = 8});
109 rcl.end();
110 chassis.moveToPose(-45, -45, 130, 1500, {.forwards = false, .minSpeed = 20});
111 chassis.turnToHeading(270, 1500, {.direction = lemlib::AngularDirection::CW_CLOCKWISE}, false);
112 distanceReset(true);
113 chassis.moveToPoint(-27, -46, 1500, {.forwards = false, .minSpeed = 40}, false);
114 chassis.tank(-0.5, 0, config, 150);
115 score_longgoal_auton(600, Color::NONE, longgoal_delay);
116 distanceReset(true, false);
117
118 //PARKING ZONE
119 matchload_state(true);
120 intake();
121 distanceReset(true, false);
122 rcl.start();
123 chassis.turnToPoint(-56.5, -46.5, 1200, { .minSpeed = 30, .earlyExitRange = 5});
124 chassis.moveToPose(-56.5, -46.5, 90, 1200, {.minSpeed = 30}, false);
125 int collision_timeout = 0;
126 while(chassis.isInMotion())
127 {
128 if(chassis.detect_collision())
129 {
130 collision_timeout += 10;
131 }
132 if(collision_timeout > 100)
133 {
134 chassis.cancelMotion();
135 }
136 pros::delay(10);
137 }
138 chassis.tank(0.3, 0, config, matchload_delay);
139 chassis.brake();
140 distanceReset(true);
141 chassis.turnToPoint(-27, -47.5, 1200, {.forwards = false, .minSpeed = 35, .earlyExitRange = 25});
142 intake_stop();
143 intakeMotor.move_velocity(200);
144 chassis.moveToPose(-27, -47.5, 270, 1200, {.forwards = false, .minSpeed = 20}, false);
145 intake_stop();
146 jamManager.enable_anti_jam(true);
147 score_longgoal_auton(600, Color::NONE, longgoal_delay);
148 rcl.end();
149 matchload_state(false);
150 distanceReset(true);
151 ltv.followPath(parkingzone_curve2, {.q_x = 4.5, .q_y = 550, .q_theta = 60});
152 ltv.waitUntil(4.5);
153 score_midgoal();
154 ltv.waitUntil(9);
155 intake_stop();
156 intake();
157 ltv.waitUntilDone();
158 intake();
159 chassis.tank(1.5, -1, config, 500);
160 chassis.brake();
161 rcl.end();
162}
163
164void left_rush()
165{
166 const float matchload_delay = 380;
167 descore.extend();
168 rcl.start();
169 chassis.setPose(-51.6, 16.5, 90);
170 chassis.moveToPoint(-26.8, 21.8, 2000, {.minSpeed = 30, .earlyExitRange = 5});
171 chassis.waitUntil(3);
172 intake();
173 chassis.waitUntil(10);
174 matchload_state(true);
175 chassis.waitUntilDone();
176 chassis.turnToPoint(-36, 42.5, 1500, {.forwards = false, .minSpeed = 30, .earlyExitRange = 15});
177 chassis.moveToPose(-36, 42.5, 140, 1000, {.forwards = false, .minSpeed = 75, .earlyExitRange = 8});
178 matchload_state(false);
179 chassis.swingToHeading(270, lemlib::DriveSide::LEFT, 200, {.minSpeed = 127, .earlyExitRange = 20});
180 chassis.turnToHeading(270, 2000, {.minSpeed = 127});
181 chassis.moveToPoint(-27, 48, 1500, {.forwards = false}, false);
182 //chassis.tank(-1, 0, config, 100);
183 score_longgoal_auton(600, allianceColor, 1000);
184 distanceReset(false, true, false, false, true);
185 chassis.moveToPose(-37, 36.6, 240, 1500, {.minSpeed = 70, .earlyExitRange = 9});
186 chassis.turnToPoint(-2.5, 37.25, 1000, {.forwards = false, .minSpeed = 55, .earlyExitRange = 15});
187 distanceReset(true);
188 chassis.moveToPose(-2.5, 37.25, 270, 1500, {.forwards = false, .minSpeed = 20});
189 chassis.waitUntil(5.5);
190 descore.retract();
191 chassis.waitUntilDone();
192 rcl.end();
193 chassis.brake();
194 chassis.turnToHeading(285, 1000, {}, false);
195}
196
197
198void right_auton_split()
199{
200 const float matchload_delay = 380;
201 descore.extend();
202 rcl.start();
203 jamManager.enable_anti_jam(false);
204 chassis.setPose(-49.7, -14, 180);
205 chassis.moveToPoint(-49.7, -45, 1500, {});
206 chassis.waitUntil(5);
207 matchload_state(true);
208 chassis.waitUntilDone();
209 chassis.turnToPoint(-69, -47, 1500, {}, false);
210 distanceReset(true);
211 intake();
212 chassis.moveToPose(-56.5, -47, 270, 1000, {.lead = 0.2, .minSpeed = 50}, false);
213 chassis.waitUntilDone();
214 chassis.tank(0.7, 0, config, 120);
215 distanceReset(true);
216 chassis.moveToPose(-28, -48, 270, 1500, {.forwards = false, .minSpeed = 60, .earlyExitRange = 5}, false);
217 score_longgoal_auton(600, allianceColor,1000);
218 distanceReset(true);
219 matchload_state(false);
220 rcl.end();
221 chassis.turnToPoint(-27, -22, 900, {}, false);
222 distanceReset(true, false, false, false, true);
223 intake();
224 chassis.moveToPoint(-24, -20, 2000, {.minSpeed = 55, .earlyExitRange = 8});
225 chassis.waitUntil(11);
226 matchload_state(true);
227 chassis.waitUntilDone();
228 distanceReset(true);
229 matchload_state(false);
230 pros::delay(100);
231 chassis.turnToPoint(-10, -8, 1200, {.minSpeed = 35, .earlyExitRange = 10});
232 chassis.moveToPose(-10, -8, 43, 1300, {.lead = 0.3});
233 matchload_state(false);
234 intake();
235 chassis.waitUntil(12);
236 matchload_state(false);
237 chassis.waitUntilDone();
238 outtake();
239 pros::delay(1500);
240 intake_stop();
241 rcl.start();
242 chassis.moveToPoint(-39, -35, 2000, {.forwards = false}, false);
243 chassis.turnToPoint(-14, -36, 2000, {.minSpeed = 45, .earlyExitRange = 30}, false);
244 distanceReset(true, true, 7);
245 chassis.moveToPose(-11.5, -36, 90,2000, {.lead = 0.2, .minSpeed = 20});
246 chassis.waitUntil(5.5);
247 descore.retract();
248 chassis.waitUntilDone();
249 pros::delay(250);
250 chassis.turnToHeading(60, 2000, {.minSpeed = 35}, false);
251 rcl.end();
252
253}
254
255void right_rush()
256{
257 descore.extend();
258 chassis.setPose(-51.6, -16.5, 90);
259 chassis.moveToPoint(-26.5, -21.8, 2000, {.minSpeed = 30, .earlyExitRange = 5});
260 chassis.waitUntil(3);
261 intake();
262 chassis.waitUntil(11);
263 matchload_state(true);
264 chassis.waitUntilDone();
265 chassis.turnToPoint(-36, -40, 1500, {.forwards = false, .minSpeed = 30, .earlyExitRange = 5});
266 chassis.moveToPose(-36, -40, 20, 1000, {.forwards = false, .minSpeed = 70, .earlyExitRange = 8});
267 chassis.swingToHeading(270, lemlib::DriveSide::RIGHT, 1500, {.minSpeed = 127, .earlyExitRange = 30});
268 chassis.turnToHeading(270, 1000, {.minSpeed = 127, .earlyExitRange = 5}, false);
269 chassis.moveToPoint(-27, -48, 1300, {.forwards = false, .minSpeed = 20}, false);
270 score_longgoal_auton(600, allianceColor, 1000);
271 distanceReset(true);
272 matchload_state(false);
273 chassis.moveToPose(-46, -59.5, 260, 1000, {.minSpeed = 60, .earlyExitRange = 7});
274 chassis.moveToPose(-4.5, -58.5, 270, 2000, {.forwards = false});
275 chassis.waitUntil(4);
276 descore.retract();
277 chassis.turnToHeading(290, 1000);
278
279 descore.retract();
280}
281
282void right_7_wing()
283{
284 descore.extend();
285 chassis.setPose(-47, -16.5, 90);
286 ltv.followPath(right_7_1, {.q_x = 6.5, .q_y = 100, .q_theta = 100, .r_ang = 0.25, .r_vel = 1});
287 intake();
288 ltv.waitUntil(19);
289 matchload_state(true);
290 ltv.waitUntilDone();
291 chassis.tank(0.55, 0, config, 410);
292 distanceReset(true);
293 rcl.start();
294 chassis.turnToPoint(-24, -47, 1000, {.forwards = false, .minSpeed = 30, .earlyExitRange = 10});
295 chassis.moveToPoint(-24, -47, 1300, {.forwards = false, .minSpeed = 60});
296 chassis.waitUntilDone();
297 score_longgoal_auton(600, allianceColor,1500);
298 distanceReset(true);
299 matchload_state(false);
300 chassis.moveToPose(-34, -60, 240, 1000, {.minSpeed = 60, .earlyExitRange = 9});
301 chassis.turnToPoint(-1.2, -59, 2000, {.forwards = false, .minSpeed = 50, .earlyExitRange = 30});
302 distanceReset(true, true, 3.5);
303 rcl.end();
304 chassis.moveToPoint(-1.2, -58.6, 2000, {.forwards = false, .minSpeed = 40});
305 chassis.waitUntil(4);
306 descore.retract();
307 chassis.waitUntilDone();
308 chassis.brake();
309 rcl.end();
310}
311
312void carry_auton() {
313 descore.extend();
314 chassis.setPose(0,0,0);
315 chassis.moveToPoint(0, 6, 10000);
316}
317
318void left_auton_split() {
319 const float starting_roll = imu.get_roll();
320 const float matchload_delay = 380;
321 descore.extend();
322 rcl.start();
323 chassis.setPose(-51.6, 16.5, 90);
324 chassis.moveToPoint(-20, 25, 2000, {.minSpeed = 30, .earlyExitRange = 5});
325 chassis.waitUntil(3);
326 intake();
327 chassis.waitUntil(10);
328 matchload_state(true);
329 chassis.waitUntilDone();
330 chassis.turnToPoint(-37.5, 42.5, 1500, {.forwards = false, .minSpeed = 40, .earlyExitRange = 15});
331 chassis.moveToPose(-37.5, 42.5, 140, 1000, {.forwards = false, .minSpeed = 75, .earlyExitRange = 8});
332 matchload_state(false);
333 chassis.swingToHeading(270, lemlib::DriveSide::LEFT, 200, {.minSpeed = 127, .earlyExitRange = 20});
334 chassis.turnToHeading(270, 2000, {.minSpeed = 127});
335 chassis.moveToPoint(-27, 48, 600, {.forwards = false}, false);
336 chassis.tank(-1, 0, config, 100);
337 score_longgoal_auton(600, allianceColor, 1200);
338 distanceReset(false, true, false, false, true);
339 chassis.turnToPoint(-58, 47, 2000, {.minSpeed = 35, .earlyExitRange = 30}, false);
340 matchload_state(true);
341 chassis.moveToPose(-58.5, 47, 270, 1500, {.lead = 0.2, .minSpeed = 20});
342 chassis.waitUntil(3);
343 intake();
344 chassis.waitUntilDone();
345 chassis.tank(35, 35);
346 pros::delay(600);
347 distanceReset(true, false);
348 chassis.tank(-2.0, 0, config, 150);
349 chassis.moveToPose(-10, 11, 315, 2000, {.forwards = false, .minSpeed = 65}, false);
350 chassis.tank(-35, -35);
351 pros::delay(100);
352 chassis.tank(-20, -20);
353 score_midgoal();
354 pros::delay(1000);
355 chassis.brake();
356 rcl.end();
357 relativeMotion(chassis.getPose().x, chassis.getPose().y, chassis.getPose().theta, 10, 1000, true, 0);
358 mid_descore.extend();
359 relativeMotion(chassis.getPose().x, chassis.getPose().y, chassis.getPose().theta, -8.5, 1000, false, 0);
360 intake_stop();
361 chassis.moveToPoint(-36, 37, 2000, {});
362 matchload_state(false);
363 chassis.turnToPoint(-10, 37.5, 2000, {.forwards = false, .minSpeed = 45, .earlyExitRange = 6});
364 distanceReset(true, true, 5);
365 chassis.moveToPose(-7, 37.5, 270,2000, {.forwards = false, .lead = 0.5});
366 chassis.waitUntil(6);
367 descore.retract();
368 chassis.waitUntilDone();
369}
370
371
372void awp_auton() {
373 int val = 0;
374 descore.extend();
375 jamManager.enable_anti_jam(false);
376 chassis.setPose(-49.7, -14, 180);
377 chassis.moveToPoint(-49.7, -45, 1500, {});
378 chassis.waitUntil(5);
379 matchload_state(true);
380 chassis.waitUntilDone();
381 chassis.turnToPoint(-69, -47, 1500, {}, false);
382 distanceReset(true);
383 intake();
384 chassis.moveToPoint(-55.5, -47, 1000, {.minSpeed = 50}, false);
385 chassis.waitUntilDone();
386 chassis.tank(0.7, 0, config, 300);
387 distanceReset(true);
388 chassis.moveToPose(-28, -46.7, 270, 1500, {.forwards = false, .minSpeed = 70, .earlyExitRange = 5}, false);
389 score_longgoal_auton(600, Color::NONE,800);
390 score_longgoal(600);
391 matchload_state(false);
392 chassis.turnToPoint(-24, -22, 2000, {.minSpeed = 127, .earlyExitRange = 10}, false);
393 intake_stop();
394 distanceReset(false, true, false, false, true);
395 ltv.followPath(awp_2, {.q_x = 7, .q_y = 450, .q_theta = 190, .r_ang = 0.2, .r_vel = 1.35});
396 intake();
397 ltv.waitUntil(5);
398 matchload_state(true);
399 ltv.waitUntil(13);
400 matchload_state(false);
401 ltv.waitUntil(46.5);
402 matchload_state(true);
403 ltv.waitUntil(62.5);
404 matchload_state(false);
405 ltv.waitUntilDone();
406 rcl.start();
407 distanceReset(false, false, true, false, true);
408 chassis.turnToPoint(-27.5, 48, 1200, {.forwards = false, .minSpeed = 70, .earlyExitRange = 30});
409 distanceReset(true, true, 2);
410 chassis.moveToPoint(-27.5, 48, 1000, {.forwards = false, .minSpeed = 60, .earlyExitRange = 7}, false);
411 matchload_state(true);
412 jamManager.set_velocities(-200, 0, -600);
413 pros::delay(100);
414 score_longgoal_auton(600, Color::NONE, 1200);
415 chassis.setPose(-27, 48, chassis.getPose().theta);
416 distanceReset(false, true, false, false, true);
417 matchload_state(true);
418 intake();
419 distanceReset(true, true, 5.5);
420 rcl.start();
421 chassis.moveToPose(-57, 46, 270, 1600, {.lead = 0.3, .minSpeed = 40}, false);
422 chassis.tank(0.46, 0, config, 550);
423 distanceReset(true, true,3.5);
424 chassis.tank(-2.0, 0, config, 150);
425 chassis.moveToPose(-10, 10, 320, 1300, {.forwards = false, .minSpeed = 75, .earlyExitRange = 3.5}, false);
426 score_midgoal();
427 chassis.tank(-20, -20);
428 pros::delay(10000);
429 rcl.end();
430
431
432}
433
434void skills_auton() {
435
436
437 descore.extend();
438 color_sort_enable = false;
439 int start;
440 int var;
441 distancePose pose;
442
443 const int longgoal_delay = 2000;
444 const int midgoal_delay = 2000;
445 const int lowgoal_delay = 2000;
446 const int matchload_delay = 2000;
447 const int mid_triball_delay = 500;
448 const int longgoal_offset = 7;
449 const int midgoal_offset = 11.7;
450 const int matchload_offset = 11.4 + 0.7;
451 const int triball_delay = 500;
452 const int dual_ball_delay = 500;
453
454 //LOWGOAL
455
456 intake_lift.retract();
457 chassis.setPose(-50, 0, 270);
458 jamManager.enable_anti_jam(false);
459 intake();
460 pros::delay(900);
461 chassis.tank(1.62, 0, config, 200);
462
463 chassis.brake();
464 chassis.tank(0.3, 0, config, 150);
465
466 pros::delay(1000);
467 chassis.tank(-0.2, 0, config, 400);
468
469 chassis.brake();
470 pros::delay(800);
471 intake_stop();
472 chassis.tank(-1.2, 0, config, 600);
473
474
475 chassis.brake();
476 chassis.turnToHeading(270, 2000, {}, false);
477
478 chassis.setPose(-44.6, 0, chassis.getPose().theta);
479 //chassis.setPose(-44.6, 0, 270);
480 distanceReset(true, false);
481 chassis.moveToPoint(-40, 0, 1000, {.forwards = false, .minSpeed = 40, .earlyExitRange = 7});
482
483 jamManager.enable_anti_jam(false);
484 chassis.turnToPoint(-25, -24, 1000, {}, false);
485
486 intakeMotor.move_velocity(600);
487 chassis.moveToPoint(-25, -24, 1500, {}, false);
488
489 jamManager.set_velocities(600, -200, 600);
490 chassis.turnToHeading(46.7, 1500, {}, false);
491
492 pros::delay(200);
493 intake_stop();
494 //chassis.moveToPoint(-12, -15 - 0.85, 1500, {}, false);
495 relativeMotion(chassis.getPose().x, chassis.getPose().y, chassis.getPose().theta, 12.5, 2000, true, 0);
496 chassis.tank(30, 30);
497 jamManager.enable_anti_jam(true);
498 intake(600);
499 pros::delay(100);
500 intake_stop();
501 intake_lift.extend();
502 jamManager.set_velocities(-90, 0, -600);
503 pros::delay(100);
504 unsigned int time_outtake = pros::millis();
505 while(pros::millis() - time_outtake < 2000)
506 {
507
508
509 if((std::abs(storageMotor.get_actual_velocity()) < 100) || (storageMotor.get_efficiency() > 7 && storageMotor.get_efficiency() < 20))
510 {
511 storageMotor.move_voltage(-12000);
512 }
513 else {
514 storageMotor.move_voltage(-8000);
515 }
516 pros::delay(10);
517 }
518 chassis.tank(25, 25);
519 pros::delay(150);
520 chassis.brake();
521 intake_stop();
522 intake_lift.retract();
523 chassis.moveToPoint(-20.5, -20.5, 1000, {.forwards = false, .minSpeed = 45, .earlyExitRange = 5});
524
525 intake_stop();
526 chassis.turnToPoint(-21.3, 5.7, 2000, { .minSpeed = 45, .earlyExitRange = 50}, false);
527
528 distanceReset(true, false, false, false, true);
529 ltv.followPath(skills_1, {.q_x = 4, .q_y = 550, .q_theta = 160, .r_ang = 0.25, .r_vel = 1.35});
530
531 score_longgoal();
532 ltv.waitUntil(20);
533 intake_stop();
534 intake();
535 ltv.waitUntil(45);
536 matchload_state(true);
537 ltv.waitUntilDone();
538 chassis.tank(50, 50);
539 pros::delay(1000);
540 storageMotor.brake();
541 intakeMotor.move_velocity(200);
542 pros::delay(1000);
543 chassis.brake();
544 distanceReset(true, false);
545 matchload_state(true);
546 intake();
547
548 rcl.start();
549 chassis.moveToPoint(-50, chassis.getPose().y, 1500, {.forwards = false, .minSpeed = 70, .earlyExitRange = 8});
550
551 chassis.moveToPose(-24,59, 270, 1500, {.forwards = false, .minSpeed = 80, .earlyExitRange = 8.5});
552
553 matchload_state(false);
554 intake_stop();
555 chassis.moveToPoint(29, 58, 1500, {.forwards = false, .minSpeed = 70});
556 chassis.moveToPose(45, 42, 320, 1500, {.forwards = false, .minSpeed = 20});
557 chassis.turnToHeading(90, 1500, {.direction = lemlib::AngularDirection::CW_CLOCKWISE}, false);
558
559 distanceReset(true, false, false, false, true);
560 chassis.moveToPose(24, 47.5, 90, 1500, {.forwards = false, .minSpeed = 20}, false);
561
562 chassis.tank(-0.65, 0, config, 300);
563 rcl.end();
564 jamManager.enable_anti_jam(true);
565
566 score_longgoal_auton(600, Color::NONE, longgoal_delay);
567
568 distanceReset(true, false);
569 matchload_state(true);
570 intake();
571
572 matchload_state(true);
573 intake();
574 rcl.start();
575 chassis.turnToPoint(56.5, 46.5, 1200, {.minSpeed = 30, .earlyExitRange = 5});
576
577 score_midgoal();
578 chassis.moveToPoint(56.5, 46.5, 2000, {.minSpeed = 20});
579
580 chassis.waitUntil(7);
581 intake_stop();
582 intake();
583 chassis.waitUntilDone();
584 int col_time = 0;
585 while(!chassis.detect_collision() && col_time < 600)
586
587 chassis.tank(30, 30);
588 col_time += 10;
589 pros::delay(10);
590 chassis.tank(30, 30);
591 pros::delay(matchload_delay);
592 chassis.brake();
593 chassis.turnToPoint(26.5, 48, 1500, {.forwards = false, .minSpeed = 25, .earlyExitRange = 15});
594
595 chassis.moveToPose(26.5, 48, 90, 1200, {.forwards = false, .minSpeed = 20});
596
597 intake_stop();
598 chassis.waitUntil(7);
599 chassis.waitUntilDone();
600 chassis.tank(-0.5, 0, config, 150);
601 jamManager.enable_anti_jam(true);
602
603 score_longgoal_auton(600, Color::NONE, longgoal_delay);
604
605 distanceReset(true);
606
607
608
609 //MIDGOAL
610 intake();
611 matchload_state(false);
612 ltv.followPath(parkingzone_curve, {.q_x = 4.5, .q_y = 550, .q_theta = 65});
613
614 score_longgoal();
615 ltv.waitUntilDone();
616 intake_stop();
617 intake();
618 rcl.end();
619
620 std::vector<std::pair<float, float>> velocityData = {
621 {0.00f, 0.137449f}, {0.02f, 0.122294f}, {0.04f, 0.117480f}, {0.06f, 0.147628f},
622 {0.08f, 0.144711f}, {0.10f, 0.139913f}, {0.12f, 0.162864f}, {0.14f, 0.190873f},
623 {0.16f, 0.203905f}, {0.18f, 0.219595f}, {0.20f, 0.240017f}, {0.22f, 0.265108f},
624 {0.24f, 0.287801f}, {0.26f, 0.312827f}, {0.28f, 0.332407f}, {0.30f, 0.355553f},
625 {0.32f, 0.383497f}, {0.34f, 0.415525f}, {0.36f, 0.441199f}, {0.38f, 0.448785f},
626 {0.40f, 0.484185f}, {0.42f, 0.522956f}, {0.44f, 0.500458f}, {0.46f, 0.539618f},
627 {0.48f, 0.584095f}, {0.50f, 0.596803f}, {0.52f, 0.592848f}, {0.54f, 0.652431f},
628 {0.56f, 0.661443f}, {0.58f, 0.679596f}, {0.60f, 0.736456f}, {0.62f, 0.815490f},
629 {0.64f, 0.898349f}, {0.66f, 0.914428f}, {0.68f, 0.945937f}, {0.70f, 1.014728f},
630 {0.72f, 1.100764f}, {0.74f, 0.987562f}, {0.76f, 0.922663f}, {0.78f, 0.891802f},
631 {0.80f, 0.892580f}, {0.82f, 0.869044f}, {0.84f, 0.939259f}, {0.86f, 0.998388f},
632 {0.88f, 1.075087f}, {0.90f, 1.002472f}, {0.92f, 0.851732f}
633 };
634
635 intake();
636 distanceReset(true);
637 for (const auto& [time, velocity] : velocityData) {
638
639 auto vel = velocity;
640 if(vel < 0.5)
641 {
642 vel = 0.65;
643 }
644 if(time <= 0.2)
645 {
646 vel = velocity * 1.65;
647 }
648 if(time >= 0.2 && time <= 0.5)
649 {
650 vel = velocity * 2.2;
651 }
652 chassis.tank(vel, 0.2, config, 20);
653 if(time >= 0.92)
654 {
655 break;
656 }
657 }
658 chassis.tank(1, 0.5, config, 700);
659 chassis.tank(0.75, 0.3, config, 450);
660 jamManager.set_velocities(200, 0, 0);
661 chassis.tank(0.8, 0.2, config, 600);
662
663 matchload_state(true);
664 u_int32_t startTime = pros::millis();
665 while (
666 distanceReset(false, false).y < -37 &&
667 frontDistance.get() > 150 &&
668 !chassis.detect_collision() &&
669 pros::millis() - startTime > 600) {
670 chassis.tank(0.6, 0.2, config, 10);
671 }
672 chassis.brake();
673 chassis.setPose(62.5, -33, chassis.getPose().theta);
674 distanceReset(true, false);
675 chassis.turnToHeading(215, 2000, {}, false);
676
677 distanceReset(true, false);
678 rcl.start();
679 chassis.moveToPoint(45, -48, 2000, {});
680
681 chassis.turnToHeading(90, 2000);
682 distanceReset(false, true, false, false, true);
683 chassis.moveToPose(26, -48, 270, 1500, {.forwards = false, .minSpeed = 25}, false);
684
685 score_longgoal_auton(600, allianceColor, 1000);
686 intake();
687 matchload_state(true);
688
689 chassis.moveToPose(56.5, -47, 90, 2000, {.minSpeed = 30}, false);
690
691 chassis.tank(0.45, 0, config, matchload_delay);
692 distanceReset(true);
693
694 rcl.start();
695 chassis.moveToPoint(50, chassis.getPose().y, 1500, {.forwards = false, .minSpeed = 80, .earlyExitRange = 8});
696
697 intake_stop();
698 intakeMotor.move_velocity(200);
699 chassis.moveToPose(24,-61, 90, 1500, {.forwards = false, .minSpeed = 30, .earlyExitRange = 7});
700
701 intake_stop();
702 matchload_state(false);
703 chassis.moveToPoint(-29, -58.5, 1500, {.forwards = false, .minSpeed = 80, .earlyExitRange = 8});
704
705 rcl.end();
706 chassis.moveToPose(-45, -45, 130, 1500, {.forwards = false, .minSpeed = 20});
707 chassis.turnToHeading(270, 1500, {.direction = lemlib::AngularDirection::CW_CLOCKWISE}, false);
708
709 distanceReset(true);
710 chassis.moveToPoint(-25.5, -46, 1300, {.forwards = false, .minSpeed = 40}, false);
711
712 chassis.tank(-0.5, 0, config, 150);
713 score_longgoal_auton(600, Color::NONE, longgoal_delay);
714 distanceReset(true, false);
715
716 //PARKING ZONE
717 matchload_state(true);
718 intake();
719 distanceReset(true, false);
720 rcl.start();
721 chassis.turnToPoint(-56.5, -46.5, 1200, { .minSpeed = 30, .earlyExitRange = 5});
722
723 chassis.moveToPose(-56.5, -46.5, 90, 1200, {.minSpeed = 30}, false);
724
725 int collision_timeout = 0;
726 while(chassis.isInMotion())
727 {
728 if(chassis.detect_collision())
729 {
730 collision_timeout += 10;
731 }
732 if(collision_timeout > 100)
733 {
734 chassis.cancelMotion();
735 }
736 pros::delay(10);
737 }
738 chassis.tank(0.3, 0, config, matchload_delay);
739
740 chassis.brake();
741 distanceReset(true);
742 chassis.turnToPoint(-25.5, -47.5, 1200, {.forwards = false, .minSpeed = 35, .earlyExitRange = 25});
743
744 intake_stop();
745 intakeMotor.move_velocity(200);
746 chassis.moveToPose(-25.5, -47.5, 270, 1550, {.forwards = false, .minSpeed = 20}, false);
747
748 intake_stop();
749 jamManager.enable_anti_jam(true);
750 score_longgoal_auton(600, Color::NONE, longgoal_delay);
751 rcl.end();
752 matchload_state(false);
753 distanceReset(true);
754
755 ltv.followPath(parkingzone_curve2, {.q_x = 4.5, .q_y = 550, .q_theta = 60});
756
757 ltv.waitUntil(4.5);
758 score_midgoal();
759 ltv.waitUntil(9);
760 intake_stop();
761 intake();
762 ltv.waitUntilDone();
763 intake();
764 chassis.tank(1.5, -1.2, config, 520);
765 chassis.brake();
766}
rcl
RCL_Controller rcl
Definition RCL.cpp:8
relativeMotion
void relativeMotion(float expected_x, float expected_y, float expected_theta, float distance, int timeout_ms, bool forw, float earlyExit)
Definition autonFunctions.cpp:123
config
const VelocityControllerConfig config
Definition autonFunctions.cpp:16
score_longgoal
void score_longgoal(int power, Color allianceColor)
Definition autonFunctions.cpp:45
intake
void intake(int power)
Definition autonFunctions.cpp:33
matchload_state
void matchload_state(bool state)
Definition autonFunctions.cpp:110
score_midgoal
void score_midgoal(int power)
Definition autonFunctions.cpp:60
intake_stop
void intake_stop(bool hood_state)
Definition autonFunctions.cpp:51
score_longgoal_auton
void score_longgoal_auton(int power, Color allianceColor, int time)
Definition autonFunctions.cpp:79
outtake
void outtake(int power)
Definition autonFunctions.cpp:39
right_auton_split
void right_auton_split()
Definition autonRoutines.cpp:198
left_auton_split
void left_auton_split()
Definition autonRoutines.cpp:318
carry_auton
void carry_auton()
Definition autonRoutines.cpp:312
ltv
LTVPathFollower ltv(config)
midgoal_delay
const int midgoal_delay
Definition autonRoutines.cpp:39
triball_delay
const int triball_delay
Definition autonRoutines.cpp:45
dual_ball_delay
const int dual_ball_delay
Definition autonRoutines.cpp:46
test_auton
void test_auton()
Definition autonRoutines.cpp:83
skills_auton
void skills_auton()
Definition autonRoutines.cpp:434
right_7_wing
void right_7_wing()
Definition autonRoutines.cpp:282
awp_auton
void awp_auton()
Definition autonRoutines.cpp:372
matchload_offset
const int matchload_offset
Definition autonRoutines.cpp:44
matchload_delay
const int matchload_delay
Definition autonRoutines.cpp:40
left_rush
void left_rush()
Definition autonRoutines.cpp:164
midgoal_offset
const int midgoal_offset
Definition autonRoutines.cpp:43
longgoal_offset
const int longgoal_offset
Definition autonRoutines.cpp:42
right_rush
void right_rush()
Definition autonRoutines.cpp:255
precompute_auton_paths
void precompute_auton_paths(std::string path_name)
Definition autonRoutines.cpp:62
longgoal_delay
const int longgoal_delay
Definition autonRoutines.cpp:38
mid_triball_delay
const int mid_triball_delay
Definition autonRoutines.cpp:41
distanceReset
distancePose distanceReset(bool setPose=false, bool filter=true, float filter_range=3.5, float heading_tolerance=40.0)
Definition distanceReset.cpp:191
chassis
Chassis chassis(drivebase, lateral_controller, angular_controller, sensors, &throttle_curve, &steer_curve)
jamManager
IntakeAntiJam jamManager(intakeMotor, outtakeMotor, storageMotor, 55)
intake_lift
pros::adi::Pneumatics intake_lift('A', false)
mid_descore
pros::adi::Pneumatics mid_descore('E', false)
storageMotor
pros::Motor storageMotor(2, pros::v5::MotorGears::blue)
color_sort_enable
bool color_sort_enable
Definition globals.cpp:104
imu
pros::Imu imu(16)
descore
pros::adi::Pneumatics descore('D', false)
allianceColor
Color allianceColor
Definition globals.cpp:103
intakeMotor
pros::Motor intakeMotor(-1, pros::v5::MotorGears::blue)
frontDistance
pros::Distance frontDistance(8)
right_7_1
const std::string right_7_1
Definition paths.cpp:8
parkingzone_curve
const std::string parkingzone_curve
Definition paths.cpp:42
awp_2
const std::string awp_2
Definition paths.cpp:18
skills_1
const std::string skills_1
Definition paths.cpp:23
parkingzone_curve2
const std::string parkingzone_curve2
Definition paths.cpp:47