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 "ltv.h"
4#include "pros/rtos.hpp"
5#include "velocityController.h"
6#include "paths.h"
7#include "ramsete.h"
8#include "distanceReset.h"
9#include "MCL.h"
10#include "colorSort.h"
11#include "crossBarrierDetection.h"
12
13/*
14
15Straight:
16KV = 5.83366315659
171.14915679465
180.277890461166
1911.8794542097
2059.3640060487
21
22Turn:
236.69921571762
241.01377617649
250.956265367397
2614.8634454339
27101.977995601
28
29*/
30
31const int longgoal_delay = 1100;
32const int midgoal_delay = 1000;
33const int matchload_delay = 500;
34const int mid_triball_delay = 500;
35const int longgoal_offset = 6;
36const int midgoal_offset = 11.7;
37const int matchload_offset = 11.9;
38const int triball_delay = 500;
39const int dual_ball_delay = 500;
40
41const VelocityControllerConfig config{
425.8432642308,
430.213526937516,
441.14429410811,
450.906356177095,
460.347072436421,
4711.4953431776,
4854.5797495382,
49};
50
51
52RamsetePathFollower ramsete(config,2.5, 0.9);
53LTVPathFollower ltv(config);
54
55void precompute_auton_paths() {
56 std::vector<std::string> paths = {};
57 ramsete.precompute_paths(paths);
58}
59
60void right_auton()
61{
62 int longgoal_y_believed = -48;
63
64 chassis.setPose(-51.25, -18.5, 180);
65 intake();
66 chassis.moveToPoint(-51.25, -51, 1000);
67 matchload_state(true);
68 chassis.waitUntilDone();
69
70 chassis.turnToPoint(-72, -51, 1000);
71 chassis.waitUntilDone();
72
73 chassis.moveToPoint(-72 + matchload_offset - 1.25, -51, 1000, {.minSpeed = 20});
74 chassis.waitUntilDone();
75 pros::delay(175);
76 chassis.turnToPoint(-22 - longgoal_offset, -51.5, 1000, {.forwards = false});
77 chassis.moveToPoint(-22 - longgoal_offset, -51.5, 1000, {.forwards = false, .minSpeed=35});
78 chassis.waitUntilDone();
79 resting_state();
80 matchload_state(false);
81 score_longgoal_auton();
82 distancePose pose = distanceReset(true);
83 longgoal_y_believed = pose.y;
84 pros::delay(longgoal_delay + 150);
85 resting_state();
86 relativeMotion(chassis.getPose().x, chassis.getPose().y, chassis.getPose().theta, 4, 200, true);
87 chassis.turnToPoint(-22, -22, 1000);
88 intake();
89 chassis.moveToPoint(-22 , -22, 1000, {.maxSpeed = 85, .minSpeed = 20});
90 chassis.waitUntil(16);
91 matchload_state(true);
92 chassis.waitUntilDone();
93
94 chassis.turnToPoint(-39, longgoal_y_believed, 1000, {.forwards = false});
95 chassis.moveToPoint(-39, longgoal_y_believed, 1500, {.forwards = false});
96 chassis.turnToPoint(-22 - longgoal_offset - 2, longgoal_y_believed, 1000, {.forwards = false});
97 chassis.moveToPoint(-22 - longgoal_offset - 2, longgoal_y_believed, 1500, {.forwards = false}, false);
98 intake_stop();
99 score_longgoal_auton();
100 pros::delay(1000);
101 resting_state(false);
102 chassis.moveToPoint(-22 - longgoal_offset - 1, longgoal_y_believed, 1000, {.forwards = false});
103 chassis.moveToPoint(-22 - longgoal_offset - 1, longgoal_y_believed, 1000, {.forwards = false});
104 score_longgoal_auton();
105 pros::delay(1000);
106
107
108}
109
110void carry_auton() {
111 chassis.setPose(-51.25, -18.5, 180);
112 distanceReset(true);
113 intake();
114 ramsete.followPath(awp_1, {.end_correction = true});
115 ramsete.waitUntil(5);
116 matchload_state(true);
117 ramsete.waitUntilDone();
118 pros::delay(matchload_delay);
119 chassis.turnToPoint(-22 - longgoal_offset, -51.5, 1000, {.forwards = false});
120 chassis.moveToPoint(-22 - longgoal_offset, -51.5, 1000, {.forwards = false, .minSpeed=35}, false);
121 intake_stop();
122 score_longgoal_auton();
123 pros::delay(longgoal_delay + 150);
124 matchload_state(false);
125 distanceReset(true);
126
127
128}
129
130void left_auton() {
131
132 chassis.setPose(-47, 12, 60);
133 intake();
134 chassis.turnToPoint(-22, 22, 1000, {});
135 chassis.moveToPoint(-22, 22 , 1000, {});
136 chassis.waitUntil(20);
137 matchload_state(true);
138 chassis.waitUntilDone();
139 pros::delay(100);
140 matchload_state(false);
141
142 ramsete.followPath(left_1, {.end_correction = true});
143 matchload_state(true);
144 ramsete.followPath(left_2, {.backwards = true});
145
146 chassis.turnToPoint(-72 + matchload_offset, 47, 1000);
147 distanceReset(true);
148 chassis.moveToPoint(-72 + matchload_offset, 47, 1500, {.forwards = true}, false);
149 pros::delay(matchload_delay);
150
151 chassis.turnToPoint(-22 - longgoal_offset, 47, 1000, {.forwards = false});
152 chassis.moveToPoint(-22 - longgoal_offset, 47, 2000, {.forwards = false, .minSpeed = 35});
153 chassis.waitUntil(10);
154 intake_stop();
155 chassis.waitUntilDone();
156 score_longgoal_auton();
157 pros::delay(longgoal_delay + 150);
158 resting_state();
159
160 ramsete.followPath(left_3, {.end_correction = true});
161 descore.retract();
162 leftMotors.move(100);
163 rightMotors.move(100);
164 pros::delay(1500);
165 leftMotors.brake();
166 rightMotors.brake();
167
168}
169
170void elim_auton() {
171 ltv.followPath(skills_test, {.test = true});
172}
173
174void awp_auton() {
175 /*
176 MCL::StartMCL(-51.25, -18.5, 180);
177 pros::Task mclTask(MCL::MonteCarlo);
178 enable_fused_odometry(true);
179 */
180 //colorSort(Color::RED);
181 //FIRST LONGGOAL
182 /*
183 MCL::StartMCL(-51.25, -18.5, 180);
184 pros::Task mclTask(MCL::MonteCarlo);
185 enable_fused_odometry(true);
186 */
187 //colorSort(Color::RED);
188
189 //FIRST LONGGOAL
190 descore.extend();
191 chassis.setPose(-51.25, -18.5, 180);
192 intake();
193 chassis.moveToPoint(-51.25, -51, 1000);
194 matchload_state(true);
195 chassis.waitUntilDone();
196
197 chassis.turnToPoint(-72, -51, 1000);
198 chassis.waitUntilDone();
199
200 chassis.moveToPoint(-72 + matchload_offset, -51, 1000, {.minSpeed = 20});
201 chassis.waitUntilDone();
202 pros::delay(50);
203
204 /*
205 matchload_state(true);
206 intake();
207 ramsete.followPath(awp_1, {.end_correction = true};
208 */
209
210 //pros::delay(matchload_delay);
211 chassis.turnToPoint(-22 - longgoal_offset, -51.5, 1000, {.forwards = false});
212 chassis.moveToPoint(-22 - longgoal_offset, -51.5, 1000, {.forwards = false, .minSpeed=35});
213 chassis.waitUntilDone();
214 resting_state();
215 matchload_state(false);
216 score_longgoal_auton();
217 distanceReset(true);
218 pros::delay(longgoal_delay + 150);
219 resting_state();
220 /*
221 relativeMotion(chassis.getPose().x, chassis.getPose().y, chassis.getPose().theta, 4, 1000, true);
222
223 //MIDDLE AUTON
224 //chassis.swingToPoint(-20 , -24, lemlib::DriveSide::RIGHT, 800);
225 chassis.turnToPoint(-24, -24, 1000);
226
227 intake();
228 chassis.moveToPoint(-24 , -24, 1000, {.maxSpeed = 85, .minSpeed = 20});
229 chassis.waitUntil(17);
230 */
231 ramsete.followPath(awp_1, {.end_correction = true});
232 ramsete.waitUntilDone();
233 matchload_state(true);
234 chassis.waitUntilDone();
235
236 chassis.turnToPoint(-24, 24, 1000);
237 chassis.waitUntilDone();
238 chassis.moveToPoint(-24, 24, 1500, {.maxSpeed = 85});
239 chassis.waitUntil(5);
240 matchload_state(false);
241 chassis.waitUntil(39);
242 matchload_state(true);
243 chassis.waitUntilDone();
244 chassis.turnToPoint(-12.3, 14.2, 1000, {.forwards = false});
245 chassis.waitUntilDone();
246 //relativeMotion(-20, 20, 133, 11.5, 1000, false);
247 //chassis.moveToPoint(-10, 11.7, 1000, {.forwards = false});
248 chassis.moveToPoint(-12.3, 14.2, 1000, {.forwards = false});
249 //chassis.moveToPose(-11, 12, 133, 850, {.forwards = false, .minSpeed = 20});
250 chassis.waitUntilDone();
251 resting_state();
252 score_midgoal();
253 pros::delay(midgoal_delay + 150);
254 intake_stop();
255 matchload_state(true);
256 intake();
257 chassis.turnToPoint(-45, 54, 2000);
258 chassis.waitUntilDone();
259 chassis.moveToPoint(-45, 54, 1200);
260 chassis.waitUntilDone();
261 trapDoor.retract();
262 matchload_state(true);
263 matchload.extend();
264 chassis.waitUntilDone();
265 chassis.turnToHeading(270, 1000);
266 chassis.waitUntilDone();
267 pros::delay(5);
268 distancePose pose = distanceReset(true);
269 std::cout << "x: " << pose.x << " y: " << pose.y << std::endl;
270 chassis.moveToPoint(-72 + matchload_offset - 2, 47.5, 1500, {.maxSpeed = 100, .minSpeed = 20});
271 chassis.waitUntilDone();
272 pros::delay(250);
273
274 chassis.turnToPoint(-22 - longgoal_offset - 2.5, 47.5, 1000, {.forwards = false});
275
276 chassis.moveToPoint(-22 - longgoal_offset - 2.5, 47.5, 2000, {.forwards = false, .minSpeed = 35});
277 chassis.waitUntil(10);
278 matchload_state(false);
279 chassis.waitUntilDone();
280 score_longgoal_auton();
281}
282
283void skills_auton() {
284 descore.extend();
285 color_sort_enable = false;
286
287 const int longgoal_delay = 1100;
288 const int midgoal_delay = 1000;
289 const int matchload_delay = 500;
290 const int mid_triball_delay = 500;
291 const int longgoal_offset = 6;
292 const int midgoal_offset = 11.7;
293 const int matchload_offset = 11.9;
294 const int triball_delay = 500;
295 const int dual_ball_delay = 500;
296 /*
297 chassis.setPose(-55, 0, 270);
298 chassis.tank(90, 90);
299 pros::delay(1200);
300 leftMotors.brake();
301 rightMotors.brake();
302 pros::delay(1000);
303 chassis.tank(-100, -100);
304 pros::delay(600);
305 leftMotors.brake();
306 rightMotors.brake();
307 chassis.turnToHeading(270, 1000);
308 chassis.waitUntilDone();
309 */
310 chassis.setPose(-46, 0, 270);
311 distancePose pose = distanceReset(true);
312 std::cout << "Believed Y from parking zone" << pose.y << std::endl;
313 chassis.setPose(pose.x, pose.y, 270);
314 chassis.turnToPoint(-24, 24, 1000);
315 chassis.moveToPoint(-24, 24, 2000);
316 chassis.turnToPoint(-12.3, 14.2, 1000, {.forwards = false});
317 chassis.moveToPoint(-12.3, 14.2, 1500, {.forwards = false}, false);
318
319 chassis.turnToPoint(-45, 54, 1000);
320 chassis.moveToPoint(-45, 54, 2000);
321 chassis.turnToPoint(-72 + matchload_offset, 47.5, 1000, {}), false;
322 distanceReset(true);
323 chassis.moveToPoint(-72 + matchload_offset, 47.5, 1500, {}, false);
324 ramsete.followPath(skills_1, {.backwards = true});
325 ramsete.waitUntilDone();
326 chassis.moveToPoint(22 + longgoal_offset, 62, 3000, {.forwards = false, .maxSpeed = 90}, false);
327 distanceReset(true);
328 chassis.turnToPoint(45, 48, 1500, {.forwards = false});
329 chassis.moveToPoint(47, 48, 2000, {.forwards = false});
330 chassis.turnToPoint(22 + longgoal_offset - 2, 48, 1000, {.forwards = false}, false);
331 distanceReset(true);
332 chassis.moveToPoint(22 + longgoal_offset - 2, 48, 1500, {.forwards = false}, false);
333 distanceReset(true);
334 chassis.turnToPoint(72 - matchload_offset, 47, 1000);
335 chassis.moveToPoint(72 - matchload_offset, 47, 2000, {}, false);
336 pros::delay(matchload_delay);
337 chassis.turnToPoint(22 + longgoal_offset - 2, 48, 1000, {.forwards = false});
338 chassis.moveToPoint(22 + longgoal_offset - 2, 48, 1500, {.forwards = false}, false);
339 pros::delay(longgoal_delay);
340 distanceReset(true);
341 ramsete.followPath(skills_2, {.backwards = false, .end_correction = true});
342 ramsete.waitUntilDone();
343 chassis.tank(100, 100);
344 pros::delay(1500);
345 chassis.turnToHeading(180, 2000);
346 chassis.waitUntilDone();
347 distanceReset(true);
348 ramsete.followPath(skills_3, {.backwards = false, .end_correction = true});
349 ramsete.waitUntilDone();
350 chassis.turnToPoint(72 - matchload_offset, -47, 1000);
351 distanceReset(true);
352 chassis.moveToPoint(72 - matchload_offset, -47, 2000, {}, false);
353 pros::delay(matchload_delay);
354
355 ramsete.followPath(skills_4, {.backwards = true, .end_correction = true});
356 ramsete.waitUntilDone();
357 chassis.moveToPoint(-22 - longgoal_offset, -62, 2000, {.forwards = true, .earlyExitRange = 2}, false);
358 ramsete.followPath(skills_5, {.backwards = false, .end_correction = true});
359 ramsete.waitUntilDone();
360 chassis.turnToPoint(-22 - longgoal_offset - 2, -47, 1000, {.forwards = false}, false);
361 distanceReset(true);
362 chassis.moveToPoint(-22 - longgoal_offset - 2, -47, 1500, {.forwards = false}, false);
363 pros::delay(longgoal_delay);
364 distanceReset(true);
365
366 chassis.turnToPoint(-72 + matchload_offset, -47, 1000);
367 chassis.moveToPoint(-72 + matchload_offset, -47, 2000, {.forwards = true}, false);
368 pros::delay(matchload_delay);
369
370 ramsete.followPath(skills_6, {.backwards = false, .end_correction = true});
371 ramsete.waitUntilDone();
372 chassis.turnToPoint(-72 + 5.5, 0, 1000);
373 chassis.moveToPoint(-72 + 5.5, 0, 2000, {.forwards = true});
374
375}
intake
void intake(int power=12000)
Definition autonFunctions.cpp:12
score_longgoal_auton
void score_longgoal_auton(int power=12000, Color allianceColor=Color::RED)
Definition autonFunctions.cpp:73
matchload_state
void matchload_state(bool state)
Definition autonFunctions.cpp:97
score_midgoal
void score_midgoal(int power=12000)
Definition autonFunctions.cpp:52
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:184
intake_stop
void intake_stop()
Definition autonFunctions.cpp:46
resting_state
void resting_state(bool trapDoor_commanded=false)
Definition autonFunctions.cpp:88
left_auton
void left_auton()
Definition autonRoutines.cpp:130
carry_auton
void carry_auton()
Definition autonRoutines.cpp:110
ramsete
RamsetePathFollower ramsete(config, 2.5, 0.9)
ltv
LTVPathFollower ltv(config)
midgoal_delay
const int midgoal_delay
Definition autonRoutines.cpp:32
config
const VelocityControllerConfig config
Definition autonRoutines.cpp:41
triball_delay
const int triball_delay
Definition autonRoutines.cpp:38
dual_ball_delay
const int dual_ball_delay
Definition autonRoutines.cpp:39
skills_auton
void skills_auton()
Definition autonRoutines.cpp:283
awp_auton
void awp_auton()
Definition autonRoutines.cpp:174
right_auton
void right_auton()
Definition autonRoutines.cpp:60
matchload_offset
const int matchload_offset
Definition autonRoutines.cpp:37
matchload_delay
const int matchload_delay
Definition autonRoutines.cpp:33
precompute_auton_paths
void precompute_auton_paths()
Definition autonRoutines.cpp:55
elim_auton
void elim_auton()
Definition autonRoutines.cpp:170
midgoal_offset
const int midgoal_offset
Definition autonRoutines.cpp:36
longgoal_offset
const int longgoal_offset
Definition autonRoutines.cpp:35
longgoal_delay
const int longgoal_delay
Definition autonRoutines.cpp:31
mid_triball_delay
const int mid_triball_delay
Definition autonRoutines.cpp:34
distanceReset
distancePose distanceReset(bool setPose=false)
Definition distanceReset.cpp:189
trapDoor
pros::adi::Pneumatics trapDoor('A', false)
color_sort_enable
bool color_sort_enable
Definition globals.cpp:94
descore
pros::adi::Pneumatics descore('D', false)
matchload
pros::adi::Pneumatics matchload('B', false)
rightMotors
pros::MotorGroup rightMotors
chassis
lemlib::Chassis chassis
leftMotors
pros::MotorGroup leftMotors
Definition globals.cpp:20
skills_2
const std::string skills_2
Definition paths.cpp:12
skills_3
const std::string skills_3
Definition paths.cpp:18
left_3
const std::string left_3
Definition paths.cpp:60
left_1
const std::string left_1
Definition paths.cpp:48
skills_5
const std::string skills_5
Definition paths.cpp:30
skills_test
const std::string skills_test
Definition paths.cpp:42
left_2
const std::string left_2
Definition paths.cpp:54
skills_6
const std::string skills_6
Definition paths.cpp:36
skills_1
const std::string skills_1
Definition paths.cpp:6
skills_4
const std::string skills_4
Definition paths.cpp:24
awp_1
const std::string awp_1
Definition paths.cpp:66