1516X High Stakes 2.0
Codebase for 1516X High Stakes season
Loading...
Searching...
No Matches
autons.cpp
Go to the documentation of this file.
1#include "robot/auton.h"
2#include "globals.h"
3#include "lemlib/chassis/chassis.hpp"
4#include "main.h" // IWYU pragma: export
5#include "pros/motors.h"
6#include "pros/rtos.hpp"
7#include "robot/intake.h"
8#include "robot/ladybrown.h"
9#include "robot/sweeper.h"
10#include <iostream>
11#include <string>
12
13using namespace Robot;
14using namespace Robot::Globals;
15
16Autonomous::AUTON_ROUTINE Autonomous::auton = RED_POS;
17std::string Autonomous::autonName = "Red Positive";
18
19constexpr int delay_constant = 1050;
20
21// Red Left
22void Autonomous::RedNeg(Intake &intake, Latch &latch, DistanceSensor &distance, LadyBrown &ladybrown) {
23
24 //
25
26 HookMotor.set_zero_position(HookMotor.get_position());
27 colorSensor.set_led_pwm(70);
28 drive_.set_brake_mode_all(pros::E_MOTOR_BRAKE_BRAKE);
29 //Edge of lady brown is 5.5 inches from the alliance wall stake.
30 chassis.setPose(0, 0, 112);
31
32 ladybrown.MoveToPoint(LadyBrown::ATTACK_STATE, 150, 900);
33
34 chassis.moveToPose(-11, 44, 180, 2000, {.forwards=false, .horizontalDrift=5.5});
35
36 ladybrown.MoveToPoint(LadyBrown::BASE_STATE, 150, 1000);
37 chassis.waitUntilDone();
38 LatchControl.extend();
39 pros::delay(250);
40 chassis.swingToPoint(-28, 54.5, lemlib::DriveSide::RIGHT, 1500, { .direction=AngularDirection::CW_CLOCKWISE });
41
42 IntakeMotor.move_velocity(600);
43 HookMotor.move_velocity(200);
44 chassis.moveToPoint(-28, 54.5, 5000);
45 chassis.turnToPoint(-42, 55, 5000);
46
47 chassis.moveToPoint(-42, 55, 2500, {.minSpeed = 75}, false);
48 pros::delay(750);
49
50 chassis.turnToPoint(-19, 40, 1000, {.forwards=false});
51 chassis.moveToPoint(-19, 40, 1000, {.forwards=false});
52 chassis.turnToPoint(-40, 39, 1000);
53 chassis.moveToPoint(-40, 39, 1000);
54
55 // chassis.turnToPoint(7.5, 39, 1000, {.direction=AngularDirection::CW_CLOCKWISE});
56 // chassis.moveToPoint(7.5, 39, 7500, {.minSpeed=25, .earlyExitRange=15});
57 // chassis.waitUntil(18);
58 // IntakeMotor.brake();
59
60 chassis.turnToPoint(9, 16.5, 1000, {.direction=AngularDirection::CW_CLOCKWISE});
61 chassis.moveToPoint(9, 16.5, 7500, {.minSpeed=25, .earlyExitRange=15});
62 chassis.waitUntil(13);
63 IntakeLift.extend();
64
65 // chassis.moveToPoint(9, 16.5, 7500, {.maxSpeed=25});
66
67}
68
69void RedNegBAK(Intake &intake, Latch &latch, DistanceSensor &distance, LadyBrown &ladybrown) {
70
71
72 //IntakeMotor.brake();
73
74 HookMotor.set_zero_position(HookMotor.get_position());
75 colorSensor.set_led_pwm(70);
76 drive_.set_brake_mode_all(pros::E_MOTOR_BRAKE_BRAKE);
77 //Edge of lady brown is 5.5 inches from the alliance wall stake.
78 chassis.setPose(0, 0, 112);
79
80 ladybrown.MoveToPoint(LadyBrown::ATTACK_STATE, 150, 900);
81
82 chassis.moveToPose(-11, 44, 180, 2000, {.forwards=false, .horizontalDrift=5.5});
83
84 ladybrown.MoveToPoint(LadyBrown::BASE_STATE, 150, 1000);
85 chassis.waitUntilDone();
86 LatchControl.extend();
87 pros::delay(250);
88 chassis.swingToPoint(-28, 53.5, lemlib::DriveSide::RIGHT, 1500, {.direction=AngularDirection::CW_CLOCKWISE});
89
90 IntakeMotor.move_velocity(600);
91 HookMotor.move_velocity(200);
92 chassis.moveToPoint(-28, 53.5, 5000);
93 chassis.turnToPoint(-43, 54, 5000);
94
95 chassis.moveToPoint(-43, 54, 2500, {.minSpeed = 65}, false);
96 pros::delay(750);
97
98 chassis.turnToPoint(-19, 40, 1000, {.forwards=false});
99 chassis.moveToPoint(-19, 40, 1000, {.forwards=false});
100 chassis.turnToPoint(-38, 39, 1000);
101 chassis.moveToPoint(-38, 39, 1000);
102
103 chassis.turnToPoint(9, 16.5, 1000, {.direction=AngularDirection::CW_CLOCKWISE});
104 chassis.moveToPoint(9, 16.5, 7500, {.minSpeed=25, .earlyExitRange=15});
105 chassis.waitUntil(10);
106 IntakeLift.extend();
107
108 chassis.moveToPoint(9, 16.5, 7500, {.maxSpeed=25});
109
110
111}
112
113// Red Right
114void Autonomous::RedPos(Intake &intake, Latch &latch, Sweeper &sweeper, DistanceSensor &distance, LadyBrown &ladybrown) {
115
116 bool elims = true;
117 drive_.set_brake_mode_all(pros::E_MOTOR_BRAKE_BRAKE);
118 chassis.setPose(0, 0,-12);
119 chassis.moveToPoint(-14, 35, 2500, {.minSpeed=45, .earlyExitRange=2});
120 IntakeMotor.move_velocity(600);
121 chassis.waitUntilDone();
122 //lesser angle due to doinker displacement
123 chassis.turnToHeading(0, 300);
124 SweeperControl.extend();
125 //Use sin for the x, cos for the y
126 chassis.moveToPoint(chassis.getPose().x-3.55, chassis.getPose().y-14, 2000, {.forwards=false, .minSpeed=25});
127 chassis.waitUntilDone();
128 SweeperControl.retract();
129 pros::delay(250);
130 // Same turn to the mogo
131 chassis.turnToHeading(-170, 1000, {.direction=AngularDirection::CCW_COUNTERCLOCKWISE});
132 std::cout << chassis.getPose().x << " + " << chassis.getPose().y << std::endl;
133 std::cout << chassis.getPose().x << " + " << chassis.getPose().y+8 << std::endl;
134
135 chassis.moveToPoint(chassis.getPose().x+8, chassis.getPose().y+21, 1500, {.forwards=false}, false);
136 LatchControl.extend();
137 pros::delay(750);
138
139 std::cout << chassis.getPose().x << " + " << chassis.getPose().y << std::endl;
140
141 //chassis.turnToPoint(29, -12, 750, {.forwards=false, .direction=AngularDirection::CCW_COUNTERCLOCKWISE});
142 chassis.moveToPoint(-29, -12, 2500, {.forwards=true}, true);
143 chassis.waitUntil(10);
144
145 HookMotor.move_velocity(200);
146 chassis.turnToPoint(-2, -14, 1000);
147 chassis.waitUntilDone();
148
149 LatchControl.retract();
150 chassis.moveToPoint(-2, -14, 2500);
151 HookMotor.move_velocity(0);
152
153 pros::delay(250);
154 chassis.moveToPose(-35, 28, -180, 2500, {.forwards=false, .lead=0.6}, false);
155
156 LatchControl.extend();
157 pros::delay(250);
158 HookMotor.move_velocity(200);
159 pros::delay(250);
160
161 if (!elims) {
162 chassis.moveToPose(-48, 38, -135, 2000, {.forwards=false, .horizontalDrift=5.5});
163 chassis.waitUntil(6);
164 HookMotor.move_velocity(0);
165 pros::Task move([&]() { ladybrown.MoveToPoint(LadyBrown::LOAD_STATE); }, "LadyBrownMove");
166
167 }
168 else {
169 pros::delay(150);
170 chassis.moveToPoint(-3.5, -3, 1000, {.minSpeed=20, .earlyExitRange=10});
171 chassis.waitUntil(4);
172 IntakeMotor.move_velocity(0);
173 }
174
175}
176// Red Sweep - PROBABLY NEVER RUN THIS
177void RedPosBAK(Intake &intake, Latch &latch, Sweeper &sweeper, DistanceSensor &distance, LadyBrown &ladybrown) {
178 drive_.set_brake_mode_all(pros::E_MOTOR_BRAKE_BRAKE);
179 chassis.setPose(0, 0,0);
180
181 // Move to first stake
182 chassis.moveToPose(0, -28, 0, 2000, {.forwards = false});
183 chassis.waitUntilDone();
184 // Grab the closest MOGO mech
185 latch.toggle();
186 pros::delay(250);
187
188 // Rotate toward blue ring
189 chassis.turnToPoint(-25, -27, 1000);
190 chassis.waitUntilDone();
191
192 // Load the ring onto the stake
193 IntakeMotor.move_velocity(600);
194 HookMotor.move_velocity(200);
195
196 // Moves to 2 ring stack
197
198 chassis.moveToPoint(-25, -27, 1250, {.forwards = true});
199 chassis.turnToPoint(-33.5, 6, 1000);
200 SweeperControl.extend();
201 chassis.moveToPoint(-35, 7, 1500);
202 chassis.waitUntilDone();
203 IntakeMotor.move_velocity(0);
204 HookMotor.move_velocity(0);
205
206 chassis.moveToPose(-25, -4, 170, 2000, {.horizontalDrift = 2});
207 chassis.moveToPoint(-32, 10, 1000, {.forwards = false});
208 chassis.waitUntilDone();
209 latch.toggle();
210
211 chassis.turnToPoint(14, -36, 800);
212 SweeperControl.retract();
213 ladybrown.MoveToPoint(LadyBrown::LOAD_STATE);
214
215 chassis.moveToPoint(14, -36, 5000);
216}
217
218
219// Blue Goal Rush
220void Autonomous::BluePos(Intake &intake, Latch &latch, Sweeper &sweeper, DistanceSensor &distance, LadyBrown &ladybrown) {
221 bool elims = false;
222 drive_.set_brake_mode_all(pros::E_MOTOR_BRAKE_BRAKE);
223 chassis.setPose(0, 0,12);
224 chassis.moveToPoint(7, 35, 2500, {.minSpeed=45, .earlyExitRange=2});
225 IntakeMotor.move_velocity(600);
226 chassis.waitUntilDone();
227 chassis.turnToHeading(-17.5, 300);
228 SweeperControl.extend();
229 //Use sin for the x, cos for the y
230 chassis.moveToPoint(chassis.getPose().x+3.55, chassis.getPose().y-14, 2000, {.forwards=false, .minSpeed=25});
231 chassis.waitUntilDone();
232 SweeperControl.retract();
233 pros::delay(250);
234
235 chassis.turnToHeading(-170, 1000, {.direction=AngularDirection::CCW_COUNTERCLOCKWISE});
236 std::cout << chassis.getPose().x << " + " << chassis.getPose().y << std::endl;
237 std::cout << chassis.getPose().x << " + " << chassis.getPose().y+8 << std::endl;
238
239 chassis.moveToPoint(chassis.getPose().x, chassis.getPose().y+21, 1500, {.forwards=false}, false);
240 LatchControl.extend();
241 pros::delay(750);
242
243 std::cout << chassis.getPose().x << " + " << chassis.getPose().y << std::endl;
244
245 //chassis.turnToPoint(29, -12, 750, {.forwards=false, .direction=AngularDirection::CCW_COUNTERCLOCKWISE});
246 chassis.moveToPoint(29, -12, 2500, {.forwards=true}, true);
247 chassis.waitUntil(10);
248
249 HookMotor.move_velocity(200);
250 chassis.turnToPoint(4, -14, 1000);
251 chassis.waitUntilDone();
252
253 LatchControl.retract();
254 chassis.moveToPoint(4, -14, 2500);
255 HookMotor.move_velocity(0);
256
257 pros::delay(250);
258 chassis.moveToPose(35, 24, -180, 2500, {.forwards=false, .lead=0.6}, false);
259
260 LatchControl.extend();
261 pros::delay(250);
262 HookMotor.move_velocity(200);
263 pros::delay(250);
264
265 if (!elims) {
266 chassis.moveToPose(48, 38, -135, 2000, {.forwards=false, .horizontalDrift=5.5});
267 chassis.waitUntil(6);
268 HookMotor.move_velocity(0);
269 pros::Task move([&]() { ladybrown.MoveToPoint(LadyBrown::LOAD_STATE); }, "LadyBrownMove");
270
271 }
272 else {
273 pros::delay(150);
274 chassis.moveToPoint(3.5, -3, 1000, {.minSpeed=20, .earlyExitRange=10});
275 chassis.waitUntil(4);
276 IntakeMotor.move_velocity(0);
277 }
278
279
280 //LatchControl.retract();
281
282 // Move to first stake
283
284
285}
286// Blue sweep - PROBABLY NEVER RUN THIS
287void BluePosBAK(Intake &intake, Latch &latch, Sweeper &sweeper, DistanceSensor &distance, LadyBrown &ladybrown) {
288
289 drive_.set_brake_mode_all(pros::E_MOTOR_BRAKE_BRAKE);
290 chassis.setPose(0, 0,0);
291
292 // Move to first stake
293 chassis.moveToPose(0, -28, 0, 2000, {.forwards = false});
294 chassis.waitUntilDone();
295 // Grab the closest MOGO mech
296 latch.toggle();
297 pros::delay(250);
298
299 // Rotate toward blue ring
300 chassis.turnToPoint(25, -27, 1000);
301 chassis.waitUntilDone();
302
303 // Load the ring onto the stake
304 IntakeMotor.move_velocity(600);
305 HookMotor.move_velocity(200);
306
307 // Moves to 2 ring stack
308
309 chassis.moveToPoint(25, -27, 1250, {.forwards = true});
310 chassis.turnToPoint(33.5, 6, 1000);
311 SweeperControl.extend();
312 chassis.moveToPoint(35, 7, 1500);
313 chassis.waitUntilDone();
314 IntakeMotor.move_velocity(0);
315 HookMotor.move_velocity(0);
316
317 chassis.moveToPose(25, -4, 170, 2000, {.horizontalDrift = 2});
318 chassis.moveToPoint(32, 10, 1000, {.forwards = false});
319 chassis.waitUntilDone();
320 latch.toggle();
321
322 chassis.turnToPoint(-14, -36, 800);
323 SweeperControl.retract();
324 ladybrown.MoveToPoint(LadyBrown::LOAD_STATE);
325
326 chassis.moveToPoint(-14, -36, 5000);
327
328}
329
330void Autonomous::BluePosLateGoalRush(Intake &intake, Latch &latch, Sweeper &sweeper, DistanceSensor &distance, LadyBrown &ladybrown) {
331
332 // drive_.set_brake_mode_all(pros::E_MOTOR_BRAKE_BRAKE);
333 // chassis.setPose(0, 0,0);
334
335 // // Move to first stake
336 // chassis.moveToPose(0, -28, 0, 2000, {.forwards = false});
337 // chassis.waitUntilDone();
338 // // Grab the closest MOGO mech
339 // latch.toggle();
340 // pros::delay(250);
341
342 // // Rotate toward blue ring
343 // chassis.turnToPoint(25, -27, 1000);
344 // chassis.waitUntilDone();
345
346 // // Load the ring onto the stake
347 // IntakeMotor.move_velocity(600);
348 // HookMotor.move_velocity(200);
349
350 // // Moves to 2 ring stack
351 // chassis.moveToPoint(25, -27, 1250, {.forwards = true});
352 // chassis.turnToPoint(25, 0, 1000, {.forwards = false});
353 // chassis.waitUntilDone();
354
355 // // Moves goal to corner
356 // chassis.moveToPoint(28, 3, 1250, {.forwards = false});
357 // chassis.waitUntilDone();
358
359 // HookMotor.brake();
360 // latch.toggle();
361
362 // // Move to Third goal
363 // chassis.moveToPoint(25, -30, 1500, {.forwards = true},true);
364 // chassis.turnToPoint(25, -40, 1000, {.forwards = false});
365 // chassis.moveToPoint(25, -38,1000, {.forwards = false});
366
367
368 drive_.set_brake_mode_all(pros::E_MOTOR_BRAKE_BRAKE);
369 //chassis.setPose(0, 0,0);
370
371 chassis.setPose(12, -9, 112);
372
373 ladybrown.MoveToPoint(LadyBrown::ATTACK_STATE, 150, 2000);
374
375 chassis.moveToPose(0, 28, -180, 2000, {.forwards=false, .horizontalDrift=5.5});
376
377 // Move to first stake
378 //chassis.moveToPose(0, -28, 0, 2000, {.forwards = false});
379
380 ladybrown.MoveToPoint(LadyBrown::BASE_STATE);
381 chassis.waitUntilDone();
382 // Grab the closest MOGO mech
383 latch.toggle();
384 pros::delay(250);
385
386 // Rotate toward blue ring
387 chassis.turnToPoint(-25, 27, 1000);
388 chassis.waitUntilDone();
389
390 // Load the ring onto the stake
391 IntakeMotor.move_velocity(600);
392 HookMotor.move_velocity(200);
393
394 // Moves to 2 ring stack
395 chassis.moveToPoint(-27, 27, 1250, {.forwards = true});
396 chassis.turnToPoint(-19, 0, 1000, {.forwards = false});
397 chassis.waitUntilDone();
398
399 // Moves goal to corner
400 chassis.moveToPoint(0, 18, 1250, {.forwards = false});
401 chassis.waitUntilDone();
402 HookMotor.brake();
403 latch.toggle();
404
405 // Move to Third goal
406 chassis.moveToPoint(-25, 26, 1500, {.forwards = true},true);
407 chassis.turnToPoint(-25, 36, 1000, {.forwards = false});
408 chassis.moveToPoint(-25, 34,1000, {.forwards = false});
409}
410
411void Autonomous::RedPosLateGoalRush(Intake &intake, Latch &latch, Sweeper &sweeper, DistanceSensor &distance, LadyBrown &ladybrown) {
412
413 drive_.set_brake_mode_all(pros::E_MOTOR_BRAKE_BRAKE);
414 //chassis.setPose(0, 0,0);
415
416 // Move to first stake
417 // chassis.moveToPose(0, -28, 0, 2000, {.forwards = false});
418 // chassis.waitUntilDone();
419 // // Grab the closest MOGO mech
420 // latch.toggle();
421 // pros::delay(250);
422
423 // // Rotate toward blue ring
424 // chassis.turnToPoint(-25, -27, 1000);
425 // chassis.waitUntilDone();
426
427 // // Load the ring onto the stake
428 // IntakeMotor.move_velocity(600);
429 // HookMotor.move_velocity(200);
430
431 // // Moves to 2 ring stack
432 // chassis.moveToPoint(-25, -27, 1250, {.forwards = true});
433 // chassis.turnToPoint(-25, 0, 1000, {.forwards = false});
434 // chassis.waitUntilDone();
435
436 // // Moves goal to corner
437 // chassis.moveToPoint(-28, 3, 1250, {.forwards = false});
438 // chassis.waitUntilDone();
439 // HookMotor.brake();
440 // latch.toggle();
441
442 // // Move to Third goal
443 // chassis.moveToPoint(-25, -30, 1500, {.forwards = true},true);
444 // chassis.turnToPoint(-25, -40, 1000, {.forwards = false});
445 // chassis.moveToPoint(-25, -38,1000, {.forwards = false});
446
447 drive_.set_brake_mode_all(pros::E_MOTOR_BRAKE_BRAKE);
448 //chassis.setPose(0, 0,0);
449
450 chassis.setPose(-12, -9, -112);
451
452 ladybrown.MoveToPoint(LadyBrown::ATTACK_STATE, 150, 1000);
453
454 chassis.moveToPose(0, 30, -180, 2000, {.forwards=false, .horizontalDrift=5.5});
455
456 // Move to first stake
457 //chassis.moveToPose(0, -28, 0, 2000, {.forwards = false});
458
459 ladybrown.MoveToPoint(LadyBrown::BASE_STATE);
460 chassis.waitUntilDone();
461 // Grab the closest MOGO mech
462 latch.toggle();
463 pros::delay(250);
464
465 // Rotate toward blue ring
466 chassis.turnToPoint(25, 27, 1000);
467 chassis.waitUntilDone();
468
469 // Load the ring onto the stake
470 IntakeMotor.move_velocity(600);
471 HookMotor.move_velocity(200);
472
473 // Moves to 2 ring stack
474 chassis.moveToPoint(27, 27, 1250, {.forwards = true});
475 chassis.turnToPoint(19, 0, 1000, {.forwards = false});
476 chassis.waitUntilDone();
477
478 // Moves goal to corner
479 chassis.moveToPoint(10, -7, 1250, {.forwards = false});
480 chassis.waitUntilDone();
481 HookMotor.brake();
482 latch.toggle();
483
484 // Move to Third goal
485 chassis.moveToPoint(25, 26, 1500, {.forwards = true},true);
486 chassis.turnToPoint(25, 36, 1000, {.forwards = false});
487 chassis.moveToPoint(25, 34,1000, {.forwards = false});
488}
489
490/*
491 * @todo Flesh out this method before the competition in order to make it a full
492 * solo awp autonomous. Blue right
493 */
494void Autonomous::BlueNeg(Intake &intake, Latch &latch, DistanceSensor &distance, LadyBrown &ladybrown) {
495
496
497 // Blue Negative 4+1 & touch
498 HookMotor.set_zero_position(HookMotor.get_position());
499 colorSensor.set_led_pwm(70);
500 drive_.set_brake_mode_all(pros::E_MOTOR_BRAKE_BRAKE);
501 //Edge of lady brown is 5.5 inches from the alliance wall stake.
502 chassis.setPose(0, 0, -112);
503
504 ladybrown.MoveToPoint(LadyBrown::ATTACK_STATE, 150, 900);
505
506 chassis.moveToPose(11, 44, 180, 2000, {.forwards=false, .horizontalDrift=5.5});
507
508 ladybrown.MoveToPoint(LadyBrown::BASE_STATE, 150, 1000);
509 chassis.waitUntilDone();
510 LatchControl.extend();
511 pros::delay(250);
512 chassis.swingToPoint(28, 54.5, lemlib::DriveSide::LEFT, 1500, {.direction=AngularDirection::CCW_COUNTERCLOCKWISE});
513
514 IntakeMotor.move_velocity(600);
515 HookMotor.move_velocity(200);
516 chassis.moveToPoint(28, 54.5, 5000);
517 chassis.turnToPoint(43, 55, 5000);
518
519 chassis.moveToPoint(43, 55, 2500, {.minSpeed = 65}, false);
520 pros::delay(750);
521
522 chassis.turnToPoint(19, 40, 1000, {.forwards=false});
523 chassis.moveToPoint(19, 40, 1000, {.forwards=false});
524 chassis.turnToPoint(38, 39, 1000);
525 chassis.moveToPoint(38, 39, 1000);
526
527 chassis.turnToPoint(-8, 16.5, 1000, {.direction=AngularDirection::CCW_COUNTERCLOCKWISE});
528 chassis.moveToPoint(-8, 16.5, 7500, {.minSpeed=25, .earlyExitRange=10});
529 chassis.waitUntil(10);
530 IntakeLift.extend();
531
532 chassis.moveToPoint(-8, 16.5, 7500, {.maxSpeed=20}, false);
533
534 //IntakeMotor.brake();
535
536
537}
538
539void BlueNegBAK(Intake &intake, Latch &latch, DistanceSensor &distance, LadyBrown &ladybrown) {
540
541
542 // Blue Negative 3+1 & touch
543 HookMotor.set_zero_position(HookMotor.get_position());
544 colorSensor.set_led_pwm(70);
545 drive_.set_brake_mode_all(pros::E_MOTOR_BRAKE_BRAKE);
546 //Edge of lady brown is 5.5 inches from the alliance wall stake.
547 chassis.setPose(0, 0, -112);
548
549 ladybrown.MoveToPoint(LadyBrown::ATTACK_STATE, 150, 900);
550
551 chassis.moveToPose(11, 44, 180, 2000, {.forwards=false, .horizontalDrift=5.5});
552
553 ladybrown.MoveToPoint(LadyBrown::BASE_STATE, 150, 1000);
554 chassis.waitUntilDone();
555 LatchControl.extend();
556 pros::delay(250);
557 chassis.swingToPoint(26, 55, lemlib::DriveSide::LEFT, 1500, {.direction=AngularDirection::CCW_COUNTERCLOCKWISE});
558
559 IntakeMotor.move_velocity(600);
560 HookMotor.move_velocity(200);
561 chassis.moveToPoint(26, 55, 5000);
562 chassis.turnToPoint(42, 55.5, 5000);
563
564 chassis.moveToPoint(42, 55.5, 2500, {.minSpeed = 65}, false);
565 pros::delay(750);
566
567 chassis.turnToPoint(32, 40, 1000, {.forwards=false});
568 chassis.moveToPoint(32, 40, 1000, {.forwards=false});
569 chassis.turnToPoint(38, 39, 1000);
570 chassis.moveToPoint(38, 39, 1000);
571
572 chassis.turnToPoint(-5, 39, 1000, {.direction=AngularDirection::CCW_COUNTERCLOCKWISE});
573 chassis.moveToPoint(-5, 39, 7500, {.minSpeed=25, .earlyExitRange=15});
574 chassis.waitUntil(18);
575 IntakeMotor.brake();
576
577
578 //IntakeMotor.brake();
579
580}
581
582// Takes in two parameters: The autonomous value as well as the puncher object.
583void Autonomous::AutoDrive(Intake &intake, Latch &latch, Sweeper &sweeper, DistanceSensor &distance,
584 LadyBrown &ladybrown) {
585 // Keep the switcher running while the controller down button has not been pressed and the time period is not
586 // autonomous Compare the current auton value to run the auton routine
587 switch (Autonomous::auton) {
588 case RED_NEG:
589 Autonomous::autonName = "Red Negative";
590 RedNeg(intake, latch, distance, ladybrown);
591 break;
592 case RED_POS:
593 Autonomous::autonName = "Red Positive";
594 RedPos(intake, latch, sweeper, distance, ladybrown);
595 break;
596 case RED_POS_LATE_RUSH:
597 Autonomous::autonName = "Red Positive Late Rush";
598 RedPosLateGoalRush(intake, latch, sweeper, distance, ladybrown);
599 break;
600 case BLUE_POS:
601 Autonomous::autonName = "Blue Positive";
602 BluePos(intake, latch, sweeper, distance, ladybrown);
603 break;
604 case BLUE_NEG:
605 Autonomous::autonName = "Blue Negative";
606 BlueNeg(intake, latch, distance, ladybrown);
607 break;
608 case BLUE_POS_LATE_RUSH:
609 Autonomous::autonName = "Blue Positive Late Rush";
610 BluePosLateGoalRush(intake, latch, sweeper, distance, ladybrown);
611 break;
612 case SKILLS:
613 Autonomous::autonName = "Skills";
614 Skills(intake, latch, distance, ladybrown);
615 break;
616 }
617}
618
619void Autonomous::AutonSwitcher(int autonNum) {
620 switch (autonNum) {
621 case 1:
622 Autonomous::autonName = "Red Negative";
623 Autonomous::auton = RED_NEG;
624 break;
625 case 2:
626 Autonomous::autonName = "Red Positive Sweep";
627 Autonomous::auton = RED_POS;
628 break;
629 case 3:
630 Autonomous::autonName = "Red Positive Late Rush";
631 Autonomous::auton = RED_POS_LATE_RUSH;
632 break;
633 case -1:
634 Autonomous::autonName = "Blue Positive Sweep";
635 Autonomous::auton = BLUE_POS;
636 break;
637 case -2:
638 Autonomous::autonName = "Blue Positive Late Rush";
639 Autonomous::auton = BLUE_POS_LATE_RUSH;
640 break;
641 case -3:
642 Autonomous::autonName = "Blue Negative";
643 Autonomous::auton = BLUE_NEG;
644 break;
645 case 0:
646 Autonomous::autonName = "Skills";
647 Autonomous::auton = SKILLS;
648 }
649 std::cout << "Current auton: " + Autonomous::autonName << std::endl;
650}
RedPosBAK
void RedPosBAK(Intake &intake, Latch &latch, Sweeper &sweeper, DistanceSensor &distance, LadyBrown &ladybrown)
Definition autons.cpp:177
BlueNegBAK
void BlueNegBAK(Intake &intake, Latch &latch, DistanceSensor &distance, LadyBrown &ladybrown)
Definition autons.cpp:539
RedNegBAK
void RedNegBAK(Intake &intake, Latch &latch, DistanceSensor &distance, LadyBrown &ladybrown)
Definition autons.cpp:69
delay_constant
constexpr int delay_constant
Definition autons.cpp:19
BluePosBAK
void BluePosBAK(Intake &intake, Latch &latch, Sweeper &sweeper, DistanceSensor &distance, LadyBrown &ladybrown)
Definition autons.cpp:287
Robot::Autonomous::BluePos
void BluePos(Intake &intake, Latch &latch, Sweeper &sweeper, DistanceSensor &distance, LadyBrown &ladybrown)
Runs the puncher routine for the Skills Challenge.
Definition autons.cpp:220
Robot::Autonomous::RedPosLateGoalRush
void RedPosLateGoalRush(Intake &intake, Latch &latch, Sweeper &sweeper, DistanceSensor &distance, LadyBrown &ladybrown)
Definition autons.cpp:411
Robot::Autonomous::RedPos
void RedPos(Intake &intake, Latch &latch, Sweeper &sweeper, DistanceSensor &distance, LadyBrown &ladybrown)
Runs the autonomous path for the near side offensive game strategy.
Definition autons.cpp:114
Robot::Autonomous::autonName
static std::string autonName
The name of the autonomous program.
Definition auton.h:35
Robot::Autonomous::BlueNeg
void BlueNeg(Intake &intake, Latch &latch, DistanceSensor &distance, LadyBrown &ladybrown)
Runs the autonomous path for the far side offensive game strategy. This function executes the autonom...
Definition autons.cpp:494
Robot::Autonomous::Skills
void Skills(Intake &intake, Latch &latch, DistanceSensor &distance, LadyBrown &ladybrown)
Definition skills.cpp:16
Robot::Autonomous::AutoDrive
void AutoDrive(Intake &intake, Latch &latch, Sweeper &sweeper, DistanceSensor &distance, LadyBrown &ladybrown)
Drives the robot autonomously.
Definition autons.cpp:583
Robot::Autonomous::BluePosLateGoalRush
void BluePosLateGoalRush(Intake &intake, Latch &latch, Sweeper &sweeper, DistanceSensor &distance, LadyBrown &ladybrown)
Definition autons.cpp:330
Robot::Autonomous::auton
static AUTON_ROUTINE auton
Sets the number of the autonomous program to use.
Definition auton.h:28
Robot::Autonomous::RedNeg
void RedNeg(Intake &intake, Latch &latch, DistanceSensor &distance, LadyBrown &ladybrown)
Runs the autonomous path for the far side defensive game strategy.
Definition autons.cpp:22
Robot::Autonomous::AutonSwitcher
static void AutonSwitcher(int autonNum)
Switches the autonomous program.
Definition autons.cpp:619
Robot::Intake
The Intake class represents a robot intake system.
Definition intake.h:8
Robot::LadyBrown
The LadyBrown class represents the robot lady brown subsystem.
Definition ladybrown.h:9
Robot::Latch
The Latch class represents a latching mechanism.
Definition latch.h:8
Robot::Latch::toggle
void toggle()
Toggles latch state.
Definition latch.cpp:16
Robot::Sweeper
The Latch class represents a latching mechanism.
Definition sweeper.h:8
ladybrown
Robot::LadyBrown ladybrown
Definition intake.cpp:19
Robot::Globals::drive_
pros::MotorGroup drive_({LeftFront.get_port(), RightFront.get_port(), LeftMid.get_port(), RightMid.get_port(), LeftBack.get_port(), RightBack.get_port()})
Robot::Globals::LatchControl
pros::adi::Pneumatics LatchControl('A', false)
Robot::Globals::colorSensor
pros::Optical colorSensor(5)
Robot::Globals::IntakeMotor
pros::Motor IntakeMotor(-4, pros::v5::MotorGears::blue, pros::v5::MotorUnits::degrees)
Robot::Globals::SweeperControl
pros::adi::Pneumatics SweeperControl('B', false)
Robot::Globals::chassis
lemlib::Chassis chassis(drivetrain, lateral_controller, angular_controller, sensors, &throttle_curve, &steer_curve)
Robot::Globals::HookMotor
pros::Motor HookMotor(3, pros::v5::MotorGears::green, pros::v5::MotorUnits::degrees)
Robot::Globals::IntakeLift
pros::adi::Pneumatics IntakeLift('D', false)
Robot
Definition auton.h:10