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#include "globals.h"
2#include "auton/autonFunctions.h"
3#include "lemlib/chassis/chassis.hpp"
4#include "ltv.h"
5#include "pros/abstract_motor.hpp"
6#include "pros/rtos.hpp"
7#include "velocityController.h"
8#include "paths.h"
9#include "ramsete.h"
10#include "distanceReset.h"
11#include "colorSort.h"
12#include "crossBarrierDetection.h"
13#include <type_traits>
14
15/*
16
17Straight:
18KV = 5.83366315659
191.14915679465
200.277890461166
2111.8794542097
2259.3640060487
23
24Turn:
256.69921571762
261.01377617649
270.956265367397
2814.8634454339
29101.977995601
30
31*/
32
33const int longgoal_delay = 1100;
34const int midgoal_delay = 1000;
35const int matchload_delay = 500;
36const int mid_triball_delay = 500;
37const int longgoal_offset = 6;
38const int midgoal_offset = 11.7;
39const int matchload_offset = 11.9;
40const int triball_delay = 500;
41const int dual_ball_delay = 500;
42
43const VelocityControllerConfig config{
445.8432642308,
450.213526937516,
461.14429410811,
470.906356177095,
480.347072436421,
4911.4953431776,
5054.5797495382,
51};
52
53RamsetePathFollower ramsete(config, 2.5, 0.7);
54LTVPathFollower ltv(config);
55
56void precompute_auton_paths() {
57 std::vector<std::string> paths = {};
58}
59
60void right_auton()
61{
62 descore.extend();
63 chassis.setPose(-44.77, -12.31, 90);
64 trapDoor.extend();
65 chassis.moveToPose(-24, -24, 145, 3000, {}); //24, 21
66 chassis.waitUntil(3);
67 trapDoor.retract();
68 intake();
69 chassis.waitUntil(21);
70 matchload_state(true);
71 chassis.waitUntilDone();
72 matchload_state(false);
73 chassis.turnToPoint(0,0, 3000,{},false);
74 intake_stop();
75 chassis.moveToPose(-11.7, -11.7, 45, 3000, {}, false);
76 outtake(7000);
77 pros::delay(2000);
78 chassis.tank(0,0);
79 intake_stop();
80 chassis.moveToPoint(-46, -47, 3500, {.forwards = false, .maxSpeed = 100});
81 chassis.waitUntil(7);
82 matchload_state(true);
83 matchload.extend();
84 chassis.waitUntilDone();
85 chassis.turnToPoint(-72, -46.5, 3500, {}, false);
86 distancePose pose = distanceReset(true, false, false, false, true);
87 intake();
88 chassis.moveToPoint(-59.2, -46.5, 700, {.minSpeed = 40}, false);
89 chassis.tank(60, 60);
90 pros::delay(120);
91 chassis.tank(0,0);
92 distanceReset(true);
93 intake_stop();
94 chassis.moveToPoint(-22 - longgoal_offset - 3.5, -46.7, 3000, {.forwards = false, .minSpeed = 15, .earlyExitRange = 1.5}, false);
95 color_sort_enable = true;
96 score_longgoal_auton(12000, allianceColor, 1700);
97 intake_stop();
98 color_sort_enable = false;
99 matchload_state(false);
100 intake_stop();
101 distanceReset(true, false, false, false, true);
102 descore.retract();
103 chassis.moveToPoint(-48, -46.5, 3000, {}, false);
104 chassis.moveToPose(-22 + longgoal_offset+1, -47.5 +12.6, 270, 2000, {.forwards = false , .lead = 0.4, .minSpeed = 20, .earlyExitRange = 8});
105 chassis.moveToPose(-16, -47.5 + 12.5, 270, 2000, {.forwards = false, .lead = 0.3});
106}
107
108void carry_auton() {
109 chassis.setPose(0,0,0);
110 chassis.moveToPoint(0, 4, 10000);
111}
112
113void left_auton() {
114 descore.extend();
115 chassis.setPose(-44.77, 12.31, 90);
116 trapDoor.extend();
117 chassis.moveToPose(-23, 23, 30, 3000, {});
118 chassis.waitUntil(3);
119 trapDoor.retract();
120 intake();
121 chassis.waitUntil(20);
122 matchload_state(true);
123 chassis.waitUntilDone();
124 chassis.turnToPoint(0,0, 1500,{.forwards = false}, true);
125 chassis.moveToPoint(-14, 14, 1500, {.forwards = false, .minSpeed = 15, .earlyExitRange = 2}, false);
126 midgoal_first = true;
127 trapDoor.extend();
128 chassis.tank(-45, -45);
129 topMotor.move_voltage(7000);
130 intakeMotor.move_voltage(12000);
131 pros::delay(300);
132 topMotor.move_velocity(350);
133 pros::delay(700);
134 chassis.tank(0,0);
135 intake_stop();
136 midgoal_first = false;
137 chassis.moveToPoint(-46, 47, 1700, {});
138 chassis.waitUntil(7);
139 trapDoor.retract();
140 chassis.waitUntilDone();
141 chassis.turnToPoint(-72, 47, 1500, {}, false);
142 distancePose pose = distanceReset(true);
143 intake();
144 chassis.moveToPoint(-59.2, 46.5, 900, {.minSpeed = 60}, false);
145 std::cout << "Position at Matchloader " << chassis.getPose().x << " " << chassis.getPose().y << std::endl;
146 chassis.tank(60, 60);
147 pros::delay(200);
148 chassis.tank(0,0);
149 chassis.turnToPoint(-22, 47.5, 1600, {.forwards = false}, false);
150 distanceReset(true);
151 chassis.moveToPoint(-22 - longgoal_offset - 3.5, 47.5, 1600, {.forwards = false, .minSpeed = 15}, false);
152 color_sort_enable = true;
153 score_longgoal_auton(12000, allianceColor, 1800);
154 intake_stop();
155 color_sort_enable = false;
156 distanceReset(true);
157 chassis.moveToPoint(-49, 47.5, 2000, {.earlyExitRange = 2});
158 distanceReset(true);
159 descore.retract();
160 chassis.waitUntilDone();
161 chassis.moveToPose(-22 + longgoal_offset + 3, 47.5 + 12.3, 270, 2000, {.forwards = false ,.lead = 0.4, .minSpeed = 20, .earlyExitRange = 8});
162 chassis.moveToPose(-16, 47.5 + 12.5, 270, 2000, {.forwards = false, .lead = 0.3});
163}
164
165void elim_auton() {
166 descore.extend();
167 chassis.setPose(-44.77, -12.31, 90);
168 trapDoor.extend();
169 chassis.moveToPose(-24, -24, 145, 3000, {}); //24, 21
170 chassis.waitUntil(3);
171 trapDoor.retract();
172 intake();
173 chassis.waitUntil(21);
174 matchload_state(true);
175 chassis.waitUntilDone();
176 matchload_state(false);
177 chassis.turnToPoint(-46, -47, 2000, {.forwards = false});
178 chassis.moveToPoint(-46, -47, 3500, {.forwards = false, .maxSpeed = 100});
179 chassis.waitUntil(7);
180 matchload_state(true);
181 matchload.extend();
182 chassis.waitUntilDone();
183 chassis.turnToPoint(-72, -46.5, 1200, {}, false);
184 distancePose pose = distanceReset(true);
185 intake();
186 chassis.moveToPoint(-59.5, -46.5, 800, {}, false);
187 chassis.tank(45, 45);
188 pros::delay(350);
189 chassis.tank(0,0);
190 chassis.turnToPoint(-22, -47.5, 2000, {.forwards = false}, false);
191 pose = distanceReset(true);
192 if(std::abs(pose.y - 47) > 2.2)
193 {
194 chassis.setPose(chassis.getPose().x, -47, chassis.getPose().theta);
195 }
196 intake_stop();
197 chassis.moveToPoint(-22 - longgoal_offset - 3.5, -47, 1600, {.forwards = false, .minSpeed = 15}, false);
198 color_sort_enable = true;
199 score_longgoal_auton(12000, allianceColor, 2700);
200 intake_stop();
201 color_sort_enable = false;
202 matchload_state(false);
203 intake_stop();
204 distanceReset(true);
205 descore.retract();
206 chassis.moveToPoint(-48, -47.5, 1500, {}, false);
207 chassis.moveToPose(-22 + longgoal_offset + 3, -47.5 + 12.3, 270, 2000, {.forwards = false , .lead = 0.4, .minSpeed = 30, .earlyExitRange = 8});
208 chassis.moveToPose(-16, -47.5 + 12, 270, 3000, {.forwards = false, .lead = 0.3});
209}
210
211void awp_auton() {
212 color_sort_enable = true;
213 descore.extend();
214 chassis.setPose(-51.8, -17.8, 180);
215 trapDoor.extend();
216 matchload_state(true);
217 chassis.moveToPoint(-51.8, -46.5, 1500, {});
218 chassis.waitUntil(3);
219 trapDoor.retract();
220 chassis.waitUntilDone();
221 chassis.turnToPoint(-72, -47, 1500, {});
222 intake();
223 chassis.moveToPoint(-59.6, -47, 1500, {.minSpeed = 40}, false);
224 chassis.tank(60, 60);
225 pros::delay(410);
226 chassis.tank(0,0);
227 distanceReset(true);
228 chassis.moveToPoint(-22 - longgoal_offset - 3, -47.3, 1250, {.forwards = false}, false);
229 score_longgoal_auton(12000, allianceColor, 1250 - 150);
230 distanceReset(true, false, false, false, true);
231 chassis.setPose(-31.2, chassis.getPose().y, chassis.getPose().theta);
232 chassis.moveToPoint(-36, -47.5, 2000, {}, false);
233 distanceReset(true);
234 intake();
235 chassis.turnToPoint(-22, -22, 2000, {.minSpeed = 30, .earlyExitRange = 15});
236 matchload_state(false);
237 chassis.waitUntilDone();
238 ltv.followPath(awp_1, {});
239 ltv.waitUntil(16);
240 matchload_state(true);
241 ltv.waitUntil(26);
242 matchload_state(false);
243 //Was 55
244 ltv.waitUntil(56);
245 matchload_state(true);
246
247 ltv.waitUntilDone();
248 std::cout << chassis.getPose().x << ", " << chassis.getPose().y << std::endl;
249 pros::delay(20);
250 distanceReset(false, true, false, false, true);
251 chassis.moveToPoint(-22 - longgoal_offset - 4.7, 47.3, 1000, {.forwards = false}, false);
252 score_longgoal_auton(12000, allianceColor, 1300);
253 distanceReset(true);
254 intake();
255 chassis.moveToPoint(-59.2, 47.3, 2000, {.minSpeed = 40}, false);
256 chassis.tank(60, 60);
257 pros::delay(350);
258 chassis.tank(0,0);
259 pros::delay(20);
260 distanceReset(true);
261 chassis.moveToPoint(-14, 14.5, 2000, {.forwards = false}, false);
262 trapDoor.extend();
263 pros::delay(50);
264 intakeMotor.move_voltage(12000);
265 topMotor.move_voltage(10000);
266 chassis.tank(-45, -45);
267 pros::delay(1000);
268 chassis.tank(0,0);
269
270}
271
272void skills_auton() {
273 descore.extend();
274 color_sort_enable = false;
275 blockBlocker.retract();
276 int start;
277 distancePose pose;
278
279 const int longgoal_delay = 2250;
280 const int midgoal_delay = 1500;
281 const int matchload_delay = 1350;
282 const int mid_triball_delay = 500;
283 const int longgoal_offset = 7;
284 const int midgoal_offset = 11.7;
285 const int matchload_offset = 11.4 + 0.7;
286 const int triball_delay = 500;
287 const int dual_ball_delay = 500;
288
289 chassis.setPose(-53, 0, 270);
290 intake();
291 pros::delay(250);
292 chassis.tank(90,90);
293 pros::delay(500);
294 chassis.tank(50,50);
295 pros::delay(700);
296 chassis.tank(0,0);
297 pros::delay(200);
298 chassis.tank(-70,-70);
299 pros::delay(550);
300 chassis.tank(90,90);
301 pros::delay(500);
302 chassis.tank(0,0);
303 pros::delay(600);
304 chassis.turnToHeading(270, 300, {}, false);
305 int base_sum = 0;
306 for(int i = 0; i < 5; i++)
307 {
308 base_sum += imu.get_roll();
309 pros::delay(10);
310 }
311 int base = base_sum / 5;
312 intake_stop();
313 chassis.tank(-127, -127);
314 intake();
315 start = pros::millis();
316 while(imu.get_roll() < base + 5.5 && (pros::millis() - start < 1000))
317 {
318 pros::delay(10);
319 }
320 start = pros::millis();
321 while(imu.get_roll() > base && (pros::millis() - start < 750))
322 {
323 pros::delay(10);
324 }
325 intake_stop();
326 chassis.tank(0,0);
327 chassis.tank(50,50);
328 pros::delay(700);
329 chassis.tank(0,0);
330 chassis.turnToHeading(270, 1000);
331 intakeFunnel.retract();
332 chassis.setPose(-45.7, 0, chassis.getPose().theta);
333 pose = distanceReset(true, false);
334 pros::delay(20);
335 chassis.moveToPoint(-45.7 + 5, 0, 2000, {.forwards = false}, false);
336 chassis.turnToPoint(0,0, 2000, {.direction = lemlib::AngularDirection::CW_CLOCKWISE}, false);
337 intakeFunnel.extend();
338 pros::delay(20);
339 distanceReset(true, false);
340 chassis.moveToPose(-22.4, 22.4, 320, 1800, {.lead = 0.5});
341 chassis.waitUntil(15);
342 intake();
343 chassis.waitUntilDone();
344 chassis.moveToPose(-14, 14.5, 320, 700, {.forwards = false, .lead = 0.2});
345 chassis.waitUntil(2);
346 intake_stop();
347 chassis.waitUntilDone();
348 std::cout << "Pose before midgoal: " << chassis.getPose().x << ", " << chassis.getPose().y << ", " << chassis.getPose().theta << std::endl;
349
350 midgoal_first = true;
351 color_sort_enable = false;
352 trapDoor.extend();
353 score_midgoal_auton();
354 matchload_state(true);
355 pros::delay(midgoal_delay);
356 matchload_state(true);
357 matchload.extend();
358 intake_stop();
359 topMotor.brake();
360 pros::delay(500);
361
362 chassis.moveToPoint(-45, 47.5, 2000, {});
363 chassis.waitUntil(3);
364 trapDoor.retract();
365 color_sort_enable = false;
366 chassis.waitUntilDone();
367 chassis.turnToPoint(-72, 45.5, 2000, {}, false);
368 pros::delay(20);
369 distanceReset(true);
370 pros::delay(20);
371 chassis.moveToPoint(-59.2, 45.5, 2000, {}, false);
372 intake();
373 chassis.tank(60,60);
374 pros::delay(matchload_delay + 500);
375 chassis.tank(0,0);
376 pros::delay(20);
377 distanceReset(true);
378
379 chassis.moveToPose(-22 + longgoal_offset, 61.5, 270, 1500, {.forwards = false, .lead = 0.3, .minSpeed = 60, .earlyExitRange = 7});
380 chassis.waitUntil(3);
381 intake_stop();
382 chassis.moveToPose(37, 61.5, 270, 1700, {.forwards = false, .lead = 0, .maxSpeed = 100});
383 chassis.waitUntil(3);
384 matchload_state(false);
385 chassis.waitUntilDone();
386 pros::delay(20);
387 distanceReset(true);
388 chassis.swingToPoint(22, 48.5, lemlib::DriveSide::LEFT, 2500, {.forwards = false});
389 chassis.moveToPoint(22 + longgoal_offset + 2, 50, 2000, {.forwards = false}, false);
390
391 //alignToGoal(90);
392 score_longgoal_auton(12000, Color::RED, longgoal_delay + 150);
393 matchload_state(true);
394 matchload.extend();
395 distanceReset(true);
396 pros::delay(20);
397 intake_stop();
398 matchload_state(true);
399 intake();
400 chassis.moveToPoint(59.2, 46.7, 2000, {.forwards = true}, false);
401 chassis.tank(60,60);
402 pros::delay(matchload_delay + 200);
403 chassis.tank(0,0);
404 chassis.turnToPoint(22 + longgoal_offset, 47.5, 1000, {.forwards = false}, false);
405 chassis.moveToPoint(22 + longgoal_offset+2, 47.5, 2000, {.forwards = false, .minSpeed = 25});
406 chassis.waitUntil(3);
407 intake_stop();
408 chassis.waitUntilDone();
409 score_longgoal_auton(12000, Color::RED, longgoal_delay + 350);
410 pros::delay(20);
411 distanceReset(true);
412
413
414 relativeMotion(chassis.getPose().x, chassis.getPose().y, 90, 6, 200, true);
415 distanceReset(true, false, false, false, true);
416 chassis.turnToPoint(45, -46.5, 2000, {}, false);
417 distanceReset(false, false, false, true, true);
418 chassis.moveToPoint(45, -46.5, 6000, {.maxSpeed = 100}, false);
419 matchload_state(true);
420
421 distanceReset(false, false, true, false, true);
422 chassis.turnToPoint(72, -46, 3000, {}, false);
423 distanceReset(true);
424
425 intake();
426 chassis.moveToPoint(59.2, -47, 1000, {.minSpeed = 40}, false);
427 chassis.tank(60, 60);
428 pros::delay(matchload_delay + 600);
429 chassis.tank(0,0);
430 pros::delay(20);
431 distanceReset(true);
432
433 intake_stop();
434 chassis.moveToPose(22 + longgoal_offset, -61.5, 90, 1500, {.forwards = false, .lead = 0.3, .minSpeed = 60, .earlyExitRange = 7});
435 chassis.moveToPose(-37, -61.5, 90, 2000, {.forwards = false, .maxSpeed = 100});
436 chassis.waitUntil(3);
437 matchload_state(false);
438 chassis.waitUntilDone();
439 pros::delay(20);
440 distanceReset(true);
441 chassis.swingToPoint(-22, -48.5, lemlib::DriveSide::LEFT, 2000, {.forwards = false}, false);
442 chassis.moveToPoint(-22 - longgoal_offset-2, -50, 1000, {.forwards = false, .minSpeed = 20}, false);
443 score_longgoal_auton(12000, Color::RED, longgoal_delay + 150);
444 matchload_state(true);
445 pros::delay(20);
446 distanceReset(true);
447 pros::delay(20);
448 intake();
449 chassis.moveToPoint(-59.2, -46.5, 2000, {}, false);
450 chassis.tank(55,55);
451 pros::delay(matchload_delay + 300);
452 chassis.tank(0,0);
453 chassis.turnToPoint(-22 - longgoal_offset, -47.5, 1000, {.forwards = false});
454 chassis.moveToPoint(-22 - longgoal_offset - 2, -47.5, 2000, {.forwards = false}, false);
455 score_longgoal_auton(12000, Color::RED, longgoal_delay + 150);
456 pros::delay(20);
457 distanceReset(true);
458 pros::delay(20);
459 matchload_state(false);
460
461 chassis.moveToPose(-62.5, -15.7, 0, 3000, {.lead = 0.3}, false);
462 intake();
463 crossBarrier(1,false, true);
464 chassis.tank(40,40);
465 pros::delay(700);
466
467 leftMotors.brake();
468 rightMotors.brake();
469}
470
471void test_auton()
472{
473 chassis.setPose(-51.25, -18.5, 180);
474 distanceReset(true);
475}
476
score_midgoal_auton
void score_midgoal_auton(int power=12000, Color allianceColor=Color::RED, int time=-1)
Definition autonFunctions.cpp:114
outtake
void outtake(int power=12000)
Definition autonFunctions.cpp:19
intake
void intake(int power=12000)
Definition autonFunctions.cpp:11
score_longgoal_auton
void score_longgoal_auton(int power=12000, Color allianceColor=Color::RED, int time=-1)
Definition autonFunctions.cpp:133
matchload_state
void matchload_state(bool state)
Definition autonFunctions.cpp:181
relativeMotion
void relativeMotion(float expected_x, float expected_y, float expected_theta, float distance, int timeout_ms, bool forw=true, float earlyExit=0)
Definition autonFunctions.cpp:217
intake_stop
void intake_stop()
Definition autonFunctions.cpp:56
left_auton
void left_auton()
Definition autonRoutines.cpp:113
carry_auton
void carry_auton()
Definition autonRoutines.cpp:108
ltv
LTVPathFollower ltv(config)
midgoal_delay
const int midgoal_delay
Definition autonRoutines.cpp:34
config
const VelocityControllerConfig config
Definition autonRoutines.cpp:43
triball_delay
const int triball_delay
Definition autonRoutines.cpp:40
dual_ball_delay
const int dual_ball_delay
Definition autonRoutines.cpp:41
test_auton
void test_auton()
Definition autonRoutines.cpp:471
skills_auton
void skills_auton()
Definition autonRoutines.cpp:272
awp_auton
void awp_auton()
Definition autonRoutines.cpp:211
right_auton
void right_auton()
Definition autonRoutines.cpp:60
matchload_offset
const int matchload_offset
Definition autonRoutines.cpp:39
ramsete
RamsetePathFollower ramsete(config, 2.5, 0.7)
matchload_delay
const int matchload_delay
Definition autonRoutines.cpp:35
precompute_auton_paths
void precompute_auton_paths()
Definition autonRoutines.cpp:56
elim_auton
void elim_auton()
Definition autonRoutines.cpp:165
midgoal_offset
const int midgoal_offset
Definition autonRoutines.cpp:38
longgoal_offset
const int longgoal_offset
Definition autonRoutines.cpp:37
longgoal_delay
const int longgoal_delay
Definition autonRoutines.cpp:33
mid_triball_delay
const int mid_triball_delay
Definition autonRoutines.cpp:36
crossBarrier
void crossBarrier(int times=2, bool reverse=false, bool fullyDrop=true)
Definition crossBarrierDetection.cpp:12
distanceReset
distancePose distanceReset(bool setPose=false, bool filter=true)
Definition distanceReset.cpp:196
blockBlocker
pros::adi::Pneumatics blockBlocker('G', false)
trapDoor
pros::adi::Pneumatics trapDoor('A', false)
intakeMotor
pros::Motor intakeMotor(18, pros::v5::MotorGears::blue)
topMotor
pros::Motor topMotor(19, pros::v5::MotorGears::blue)
color_sort_enable
bool color_sort_enable
Definition globals.cpp:97
matchload
pros::adi::Pneumatics matchload('E', false)
descore
pros::adi::Pneumatics descore('D', false)
allianceColor
Color allianceColor
Definition globals.cpp:96
intakeFunnel
pros::adi::Pneumatics intakeFunnel('B', true)
midgoal_first
bool midgoal_first
Definition globals.cpp:98
rightMotors
pros::MotorGroup rightMotors
imu
pros::IMU imu
chassis
lemlib::Chassis chassis
leftMotors
pros::MotorGroup leftMotors
Definition globals.cpp:22
awp_1
const std::string awp_1
Definition paths.cpp:77