From 1e2eb100698493ec94e41a1724aed5cb954cdf8c Mon Sep 17 00:00:00 2001 From: Derock Date: Sat, 2 Mar 2024 08:35:29 -0500 Subject: [PATCH] feat: new defense auton, and tons of fixes --- include/robot/odom.hpp | 10 ++ src/main.cpp | 218 +++++++++++++++++++++++------------- src/odom/movement.cpp | 43 +++++-- src/screen/auton.cpp | 4 + src/subsystems/catapult.cpp | 2 +- 5 files changed, 192 insertions(+), 85 deletions(-) diff --git a/include/robot/odom.hpp b/include/robot/odom.hpp index e7a8554..96e1084 100644 --- a/include/robot/odom.hpp +++ b/include/robot/odom.hpp @@ -126,6 +126,16 @@ void setChassisBrake(pros::motor_brake_mode_e_t mode); */ void holdAngle(double angle); +/** + * Waits until the robot is settled + */ +void waitUntilSettled(uint timeout = 10'000); + +/** + * Waits until a certain distance is reached + */ +void waitUntilDistance(double distance, uint timeout = 10'000); + /** * Autonomous enum */ diff --git a/src/main.cpp b/src/main.cpp index ab5e393..7cd6072 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -60,34 +60,38 @@ void competition_initialize() {} void autonomous() { odom::RobotPosition start = odom::getPosition(); - std::cout << "starting" << std::endl; - odom::MoveToPoseParams pushParams = { + const odom::MoveToPoseParams pushParams = { .chasePower = 100, .slew = 127, .exitOnStall = true}; - odom::MoveToPoseParams pushParamsWithCurve = {.chasePower = 100, - .lead = 1, - .slew = 127, - .forwards = false, - .exitOnStall = true}; + const odom::MoveToPoseParams pushParamsWithCurve = {.chasePower = 100, + .lead = 0.3, + .slew = 127, + .forwards = false, + .exitOnStall = true}; // SKILLS switch (odom::autonomous) { case odom::Autonomous::Skills: { // move to launch position - odom::moveDistance(-40, 1'000, - {.chasePower = 500, .slew = 127, .exitOnStall = true}); + odom::moveTo(-54, -30, 180, 2'000, + {.chasePower = 127, + .lead = 0.3, + .slew = 127, + .forwards = false, + .exitOnStall = true}); odom::moveDistance(11, 1'000); - odom::turnTo(71.2); + odom::turnTo(69.2); + odom::startChainedMovement(1); odom::moveDistance(-7, 1'000); + wings_left->extend(); // fire - wings_left->extend(); odom::setChassisBrake(pros::E_MOTOR_BRAKE_HOLD); - odom::holdAngle(71.2); + odom::holdAngle(69.2); odom::move(-20, -20); catapult::rapidFire = true; catapult::fire(); - // pros::delay(30'000); - pros::delay(5'000); + pros::delay(30'000); + // pros::delay(5'000); catapult::rapidFire = false; odom::move(0, 0); odom::setChassisBrake(pros::E_MOTOR_BRAKE_COAST); @@ -103,14 +107,14 @@ void autonomous() { blocker_1->toggle(); blocker_2->toggle(); }); - odom::startChainedMovement(255); // chain for rest of time because - // accuracy not super important here. + odom::moveTo(-33, -63, 90, 2'500, {.chasePower = 5, .lead = 0.55}, false); // toggle wings odom::moveTo(25, -63, 90, 5'000, {.chasePower = 10, .lead = 0.1}, false); odom::turnTo(255); - odom::moveTo(60, -22, 180, 2'500, + wings_left->extend(); + odom::moveTo(62, -22, 180, 2'500, {.chasePower = 20, .lead = 0.43, .slew = 127, @@ -118,33 +122,55 @@ void autonomous() { .exitOnStall = true}, false); // left push - odom::moveDistance(12); - odom::turnTo(120); - odom::moveDistance(-20); - wings_left->toggle(); - - odom::moveTo(50, -1.3, 270, 3'000, pushParamsWithCurve); - // odom::moveTo(1, 40, ) - odom::moveDistance(30, 2'500); - wings_left->toggle(); - odom::turnTo(180); - odom::moveDistance(-50, 2'500); - odom::turnTo(300); - wings_left->toggle(); - odom::moveDistance(-30, 1'500, pushParams); - odom::moveDistance(30, 2'500); - wings_left->toggle(); - odom::turnTo(0); - odom::moveDistance(-30, 2'500); + odom::moveDistance(6); + wings_left->retract(); odom::turnTo(270); - wings_left->toggle(); - intake_motor->move(-127); - odom::moveDistance(-30, 1'500, pushParams); - odom::moveDistance(30, 2'500); + odom::startChainedMovement(255); // chain for rest of time because + // accuracy not super important here. + odom::moveTo(12, -28, 270, 2'500, {.chasePower = 10, .lead = 0}, false); + odom::turnTo(210); + wings_left->extend(); + wings_right->extend(); + odom::moveTo(42, -6, 270, 2'500, pushParamsWithCurve, false); + + // back up for push 2 + odom::moveDistance(5, 1'000); + wings_left->retract(); + wings_right->retract(); + odom::moveDistance(10, 1'000); + + // position for push 2 + odom::moveTo(10, 40, 0, 5'000, + {.chasePower = 20, .lead = 0.3, .exitOnStall = true}, false); + odom::turnTo(325); + + // push! + wings_left->extend(); + wings_right->extend(); + odom::moveTo(45, 4, 270, 5'000, pushParamsWithCurve, false); + odom::moveDistance(5, 1'000); + wings_left->retract(); + wings_right->retract(); + + // last stretch + odom::moveTo(12, 10, 270, 2'000, + {.chasePower = 20, .lead = 0.3, .forwards = false}, false); + odom::turnTo(212); + wings_left->extend(); + wings_right->extend(); + odom::moveTo(39, 43, 250, 5'000, + {.chasePower = 20, .lead = 0.4, .forwards = false}, false); + wings_right->retract(); + odom::moveTo(60, 40, 0, 5'000, + {.chasePower = 20, .lead = 0.3, .forwards = false}, false); + odom::moveDistance(-20, 1'000, pushParams); + odom::moveDistance(5, 1'000); + wings_left->retract(); + } break; case odom::Autonomous::SixBall: { - // activate intake + // Gets the ball under the intake and releases intake from stored position blocker_1->toggle(); blocker_2->toggle(); pros::delay(300); @@ -153,18 +179,30 @@ void autonomous() { blocker_1->toggle(); blocker_2->toggle(); intake_motor->move(40); + // --- - // descore corner thingy - // odom::moveDistance(33, 3'000, {.slew = 10}); + // Lines up to descore corner ball odom::moveTo(18.725, -63.547, 90, 3'000, {.chasePower = 10, .lead = 0, .slew = 127}); odom::turnTo(220, 2'000, 70); // TODO: ensure only turns right + // ---- + + // Descores corner ball wings_right->extend(); - odom::moveTo( - 32, -52.38, 220, 3'000, - {.chasePower = 20, .lead = 0.3, .slew = 127, .forwards = false}); - odom::turnTo(150); + odom::startChainedMovement(5); + odom::moveTo(32.5, -52.38, 220, 3'000, + {.chasePower = 30, + .lead = 0.3, + .slew = 127, + .forwards = false, + .exitOnStall = true}); // CHAINED 1 + // small turn to ensure descored + odom::turnTo(150); // CHAINED 2 + // --- + + // Does a push to ensure all balls scored in goal wings_right->retract(); + wings_left->extend(); intake_motor->move(-127); odom::moveTo(39, -30.38, 180, 3'000, {.chasePower = 20, @@ -172,31 +210,21 @@ void autonomous() { .slew = 127, .forwards = false, .exitOnStall = true, - .stallThreshold = 2}); + .stallThreshold = 2}); // CHAINED 3 - odom::moveDistance(5); - odom::moveDistance(-5, 1'000, - {.chasePower = 500, .slew = 127, .exitOnStall = true}); + odom::moveDistance(5); // CHAINED 4 + odom::moveDistance(-5, 1'000, pushParams); // CHAINED 5 + wings_left->retract(); + // --- - // push 2 - // wings_left->retract(); + // line up for middle ball odom::moveDistance(20, 1'000); - // odom::moveDistance(8, 1'000); - // odom::turnTo(20); - // odom::moveDistance(15, 1'000); - // odom::moveDistance(-14, 1'000); - - // 17,-53 - // 45,40 - - // -25, -6 + // go to ball double angleToBall1 = utils::radToDeg(utils::angleSquish( odom::getPosition(false, false).angle({-10, -8, 0}))); - // odom::turnTo(angleToBall1, 2'000); intake_motor->move(60); - // odom::moveDistance(-63, 2'000); odom::moveTo(-10, -8, angleToBall1, 2'000, {.chasePower = 10, .slew = 4, .forwards = false}, false); odom::turnTo(270, 2'500, 60); @@ -205,18 +233,15 @@ void autonomous() { intake_motor->move(-127); odom::moveDistance(-30, 1'000, {.slew = 127, .exitOnStall = true}); odom::moveDistance(5, 1'000); - - // -22, -44 - // double angleToPole = utils::radToDeg(utils::angleSquish( - // odom::getPosition(false, false).angle({-22, -46, 0}))); - intake_motor->move(0); odom::turnTo(216, 2'000); wings_left->retract(); wings_right->retract(); + + // touch bar blocker_1->extend(); blocker_2->extend(); - odom::moveDistance(50, 1'000); + odom::moveDistance(50, 1'000); // TODO: absolute } break; @@ -226,7 +251,43 @@ void autonomous() { } break; case odom::Autonomous::Defense: { - // TODO + // nav to middle ball + odom::startChainedMovement(2); + odom::moveDistance(-6, 500); + odom::moveTo( + -55, 0, 196, 5'000, + {.chasePower = 5000, .lead = -0.15, .slew = 127, .forwards = false}, + true); + + // release intake + blocker_1->extend(); + blocker_2->extend(); + pros::delay(100); + intake_motor->move(127); + pros::delay(200); + blocker_1->retract(); + blocker_2->retract(); + odom::waitUntilSettled(5'000); + pros::delay(100); + // intake_motor->move(60 /); + + // go back to start + odom::moveTo(-88, -28, 305, 2'000, {.lead = 0.3}); + wings_right->extend(); + odom::moveDistance(-16, 2'000); + odom::turnTo(270); + wings_right->retract(); + + intake_motor->move(-60); + odom::moveTo(-37, -39, 270, 5'000, + {.chasePower = 8, .lead = 0.1, .forwards = false}); + blocker_1->extend(); + blocker_2->extend(); + wings_left->extend(); + pros::delay(500); + blocker_1->retract(); + blocker_2->retract(); + } break; default: @@ -313,12 +374,18 @@ void opcontrol() { master.get_digital(DIGITAL_L1) * 127); // wings - if (master.get_digital_new_press(DIGITAL_R2)) { - wings_right->toggle(); - } + if (master.get_digital_new_press(DIGITAL_R2) || + master.get_digital_new_press(DIGITAL_L2)) { + bool extended = wings_left->is_extended() || wings_right->is_extended(); - if (master.get_digital_new_press(DIGITAL_L2)) { - wings_left->toggle(); + if (extended) { + // retract + wings_left->retract(); + wings_right->retract(); + } else { + wings_left->extend(); + wings_right->extend(); + } } // blocker @@ -329,6 +396,7 @@ void opcontrol() { if (master.get_digital_new_press(DIGITAL_LEFT)) { dtReversed = !dtReversed; + master.rumble(dtReversed ? "-" : "."); } catapult::ensureTask(); diff --git a/src/odom/movement.cpp b/src/odom/movement.cpp index 83def8f..c299e98 100644 --- a/src/odom/movement.cpp +++ b/src/odom/movement.cpp @@ -3,10 +3,11 @@ #include "robot/utils.hpp" int chainedMovementCount = 0; +float distanceTravelled = -1; odom::Autonomous odom::autonomous = odom::Autonomous::None; std::shared_ptr odom::turnPID = - std::make_shared(5, 0, 20); + std::make_shared(5, 0, 25); std::shared_ptr odom::drivePID = std::make_shared(32, 0, 20); @@ -142,11 +143,15 @@ void odom::moveTo(float x, float y, float theta, int timeout, return; } + // update settlement behavior for chained PID movement updateSettlement(); if (chainedMovementCount > 0) { chainedMovementCount--; } + // set initial distance travelled + distanceTravelled = 0; + // reset PIDs and exit conditions drivePID->reset(); lateralLargeExit->reset(); @@ -167,7 +172,6 @@ void odom::moveTo(float x, float y, float theta, int timeout, // initialize vars used between iterations RobotPosition lastPose = getPosition(); - int distTravelled = 0; utils::Timer timer(timeout); bool close = false; bool lateralSettled = false; @@ -185,7 +189,7 @@ void odom::moveTo(float x, float y, float theta, int timeout, RobotPosition pose = getPosition(false, true); // stall detection - if (params.exitOnStall && distTravelled > 0) { + if (params.exitOnStall && distanceTravelled > 0) { // require to be at least 50% of the way to the target double distanceToTarget = pose.distance(target); double distanceTravelled = pose.distance(startingPosition); @@ -200,7 +204,7 @@ void odom::moveTo(float x, float y, float theta, int timeout, } // update distance travelled - distTravelled += pose.distance(lastPose); + distanceTravelled += pose.distance(lastPose); lastPose = pose; // calculate distance to the target point @@ -220,7 +224,6 @@ void odom::moveTo(float x, float y, float theta, int timeout, RobotPosition carrot = target - RobotPosition(cos(target.theta), sin(target.theta), 0) * params.lead * distTarget; - printf("carrot: %f, %f\n", carrot.x, carrot.y); if (close) carrot = target; // settling behavior @@ -266,8 +269,6 @@ void odom::moveTo(float x, float y, float theta, int timeout, float lateralError = pose.distance(carrot); - // printf("distance: %f\n", lateralError); - // only use cos when settling // otherwise just multiply by the sign of cos // maxSlipSpeed takes care of lateralOut @@ -277,8 +278,6 @@ void odom::moveTo(float x, float y, float theta, int timeout, else lateralError *= utils::sgn(cosTheta); - printf("lateralError: %f\n", lateralError); - // update exit conditions lateralSmallExit->update(lateralError); lateralLargeExit->update(lateralError); @@ -345,12 +344,21 @@ void odom::moveTo(float x, float y, float theta, int timeout, // stop the drivetrain move(0, 0); + + // reset distancedTravelled to indicate we are done + distanceTravelled = -1; } /** * @brief Turns to a given angle */ void odom::turnTo(double degrees, double timeout, double maxSpeed) { + // check chained movement + updateSettlement(); + if (chainedMovementCount > 0) { + chainedMovementCount--; + } + // reset angular controllers/exit conditions turnPID->reset(); angularLargeExit->reset(); @@ -390,4 +398,21 @@ void odom::turnTo(double degrees, double timeout, double maxSpeed) { void odom::startChainedMovement(int amount) { chainedMovementCount = amount; updateSettlement(); +} + +void odom::waitUntilSettled(uint timeout) { + utils::Timer timer(timeout); + + while (distanceTravelled != -1 && !timer.isUp()) { + pros::delay(20); + } +} + +void odom::waitUntilDistance(double distance, uint timeout) { + utils::Timer timer(timeout); + + while (distanceTravelled < distance && distanceTravelled != -1 && + !timer.isUp()) { + pros::delay(20); + } } \ No newline at end of file diff --git a/src/screen/auton.cpp b/src/screen/auton.cpp index a4a6f56..97282a8 100644 --- a/src/screen/auton.cpp +++ b/src/screen/auton.cpp @@ -57,6 +57,10 @@ static void reset_position_event_cb(lv_event_t *event) { {-8, -70 + (double)DRIVE_TRACK_WIDTH / 2, utils::degToRad(90)}); break; + case odom::Autonomous::Defense: + odom::reset({-60, -40, M_PI}); + break; + default: odom::reset({0, 0, 0}); break; diff --git a/src/subsystems/catapult.cpp b/src/subsystems/catapult.cpp index d8f78a4..c89c443 100644 --- a/src/subsystems/catapult.cpp +++ b/src/subsystems/catapult.cpp @@ -11,7 +11,7 @@ catapult::CatapultState catapult::catapultState = RELOADING; pros::Task *catapult::catapultTask = nullptr; bool catapult::rapidFire = false; -#define FIRE_SPEED 127 +#define FIRE_SPEED 140 /** * A simple state machine-like function to handle catapult control.