From b3348772fc9f9758466192b9f95842fff03742b9 Mon Sep 17 00:00:00 2001 From: Ryan McGee Date: Sun, 24 Mar 2024 18:17:44 -0400 Subject: [PATCH] Started work on new climb code --- .vscode/vex_project_settings.json | 2 +- include/cata/cata.h | 11 +- include/cata/common.h | 10 +- include/cata_system.h | 3 + src/cata/cata.cpp | 86 ++++++++++++++++ src/cata_system.cpp | 9 ++ src/competition/opcontrol.cpp | 165 ++++++++++++++++-------------- 7 files changed, 200 insertions(+), 86 deletions(-) diff --git a/.vscode/vex_project_settings.json b/.vscode/vex_project_settings.json index f0006c5..d1ef000 100755 --- a/.vscode/vex_project_settings.json +++ b/.vscode/vex_project_settings.json @@ -9,7 +9,7 @@ "creationDate": "Sun, 10 Mar 2024 19:16:09 GMT", "platform": "V5", "language": "cpp", - "slot": 1, + "slot": 8, "sdkVersion": "V5_20220726_10_00_00", "cpp": { "includePath": [], diff --git a/include/cata/cata.h b/include/cata/cata.h index fe131c0..43e5301 100644 --- a/include/cata/cata.h +++ b/include/cata/cata.h @@ -13,19 +13,24 @@ enum class CataOnlyMessage { StartDrop, EnableCata, DisableCata, - + StartClimb, + ClimbReleaseMotors, + StopClimb, + FinishClimb, }; -enum class CataOnlyState { CataOff, WaitingForDrop, Firing, Reloading, ReadyToFire }; +enum class CataOnlyState { CataOff, WaitingForDrop, Firing, Reloading, ReadyToFire, PrimeClimb, ClimbHold }; std::string to_string(CataOnlyState s); std::string to_string(CataOnlyMessage s); -class CataOnlySys : public StateMachine { +class CataOnlySys : public StateMachine { public: friend struct Reloading; friend class Firing; friend class ReadyToFire; friend class WaitingForDrop; friend class CataOff; + friend class PrimeClimb; + friend class ClimbHold; friend class CataSysPage; CataOnlySys(vex::pot &cata_pot, vex::optical &cata_watcher, vex::motor_group &cata_motor, PIDFF &cata_pid, diff --git a/include/cata/common.h b/include/cata/common.h index 226bc7e..517d3b7 100644 --- a/include/cata/common.h +++ b/include/cata/common.h @@ -5,8 +5,8 @@ enum class DropMode { }; // Intake Params -const double inake_enable_lower_threshold = 90; -const double intake_enable_upper_threshold = 110; +const double inake_enable_lower_threshold = 20; +const double intake_enable_upper_threshold = 30; const double intake_upper_outer_volt = 12.0; const double intake_lower_outer_volt = 9.0; @@ -22,8 +22,10 @@ const double intake_sensor_dist_mm = 150; const double intake_drop_seconds = 0.5; // Cata Params -const double cata_target_charge = 100; -const double done_firing_angle = 110; +const double cata_target_charge = 22; +const double cata_target_climb_primed = 48; +const double cata_target_climb_up = 25; +const double done_firing_angle = 50; const double intake_drop_seconds_until_enable = 0.25; const double fire_voltage = 12.0; diff --git a/include/cata_system.h b/include/cata_system.h index 7f630f7..960a029 100644 --- a/include/cata_system.h +++ b/include/cata_system.h @@ -18,6 +18,9 @@ class CataSys { StartDropping, StartFiring, ToggleCata, + StartClimb, + StopClimb, + FinishClimb, }; CataSys( diff --git a/src/cata/cata.cpp b/src/cata/cata.cpp index 8f24a32..004715f 100644 --- a/src/cata/cata.cpp +++ b/src/cata/cata.cpp @@ -99,11 +99,62 @@ class ReadyToFire : public CataOnlySys::State { CataOnlyState id() const override { return CataOnlyState::ReadyToFire; } }; +class PrimeClimb : public CataOnlySys::State { + public: + + void entry(CataOnlySys &sys) override { + sys.pid.set_target(cata_target_climb_primed); + }; + + CataOnlySys::MaybeMessage work(CataOnlySys &sys) override { + sys.pid.update(sys.pot.angle(vex::deg)); + sys.mot.spin(vex::fwd, sys.pid.get(), vex::volt); + + return {}; + }; + + void exit(CataOnlySys &sys) override { + + }; + + CataOnlyState id() const override { + return CataOnlyState::PrimeClimb; + }; + + CataOnlySys::State *respond(CataOnlySys &s, CataOnlyMessage m) override; +}; + +class ClimbHold : public CataOnlySys::State { + public: + void entry(CataOnlySys &sys) override { + sys.pid.set_target(cata_target_climb_up); + }; + + CataOnlySys::MaybeMessage work(CataOnlySys &sys) override { + sys.pid.update(sys.pot.angle(vex::deg)); + sys.mot.spin(vex::fwd, sys.pid.get(), vex::volt); + + return {}; + }; + + void exit(CataOnlySys &sys) override { + + }; + + CataOnlyState id() const override { + return CataOnlyState::ClimbHold; + }; + + CataOnlySys::State *respond(CataOnlySys &s, CataOnlyMessage m) override; +}; + CataOnlySys::State *CataOff::respond(CataOnlySys &sys, CataOnlyMessage m) { if (m == CataOnlyMessage::EnableCata) { return new Reloading(); } else if (m == CataOnlyMessage::StartDrop) { return new WaitingForDrop(); + } else if (m == CataOnlyMessage::StartClimb) { + return new PrimeClimb(); } // Ignore other messages return this; @@ -128,6 +179,8 @@ CataOnlySys::State *Reloading::respond(CataOnlySys &sys, CataOnlyMessage m) { } else { return this; } + } else if (m == CataOnlyMessage::StartClimb) { + return new PrimeClimb(); } // Ignore other messages return this; @@ -144,6 +197,8 @@ CataOnlySys::State *ReadyToFire::respond(CataOnlySys &sys, CataOnlyMessage m) { return new Reloading(); } else if (m == CataOnlyMessage::DisableCata) { return new CataOff(); + } else if (m == CataOnlyMessage::StartClimb) { + return new PrimeClimb(); } // Ignore other messages @@ -159,6 +214,27 @@ CataOnlySys::State *Firing::respond(CataOnlySys &sys, CataOnlyMessage m) { // Ignore other messages return this; } + +CataOnlySys::State *PrimeClimb::respond(CataOnlySys &s, CataOnlyMessage m) { + switch(m) { + case CataOnlyMessage::StopClimb: + return new Reloading(); + case CataOnlyMessage::FinishClimb: + return new ClimbHold(); + default: + return this; + } +}; + +CataOnlySys::State *ClimbHold::respond(CataOnlySys &s, CataOnlyMessage m) { + switch(m) { + case CataOnlyMessage::StopClimb: + return new Reloading(); + default: + return this; + } +}; + std::string to_string(CataOnlyState s) { switch (s) { case CataOnlyState::Firing: @@ -171,6 +247,10 @@ std::string to_string(CataOnlyState s) { return "CataOff"; case CataOnlyState::WaitingForDrop: return "WaitingForDrop"; + case CataOnlyState::PrimeClimb: + return "PrimeClimb"; + case CataOnlyState::ClimbHold: + return "ClimbHold"; default: return "UNKNOWN CATA STATE"; } @@ -193,6 +273,12 @@ std::string to_string(CataOnlyMessage m) { return "DisableCata"; case CataOnlyMessage::StartDrop: return "StartDrop"; + case CataOnlyMessage::StartClimb: + return "StartClimb"; + case CataOnlyMessage::StopClimb: + return "StopClimb"; + case CataOnlyMessage::FinishClimb: + return "FinishClimb"; } return "UNHANDLED CATA MESSAGE"; } diff --git a/src/cata_system.cpp b/src/cata_system.cpp index abf564f..7bbbd5e 100644 --- a/src/cata_system.cpp +++ b/src/cata_system.cpp @@ -49,6 +49,15 @@ void CataSys::send_command(Command next_cmd) { cata_sys.send_message(CataOnlyMessage::DisableCata); } break; + case CataSys::Command::StartClimb: + cata_sys.send_message(CataOnlyMessage::StartClimb); + break; + case CataSys::Command::StopClimb: + cata_sys.send_message(CataOnlyMessage::StopClimb); + break; + case CataSys::Command::FinishClimb: + cata_sys.send_message(CataOnlyMessage::FinishClimb); + break; default: break; } diff --git a/src/competition/opcontrol.cpp b/src/competition/opcontrol.cpp index 953e026..ec8f322 100644 --- a/src/competition/opcontrol.cpp +++ b/src/competition/opcontrol.cpp @@ -9,24 +9,11 @@ #define Tank -/** - * Main entrypoint for the driver control period - */ -void opcontrol() { - // ================ TUNING CODE (Disable when not testing) ================ - // testing(); - - // ================ INIT ================ - // Ensure the catapult system is enabled during driver control - if (cata_sys.get_cata_state() == CataOnlyState::CataOff) { - cata_sys.send_command(CataSys::Command::ToggleCata); - } - - static bool enable_matchload = false; - - left_wing.set(false); - right_wing.set(false); +std::atomic disable_drive(false); +std::atomic brake_mode_toggled(false); +void setupJoeControls() +{ // Controls: // Catapult: // -- L2 - Fire (Don't if ball isn't detected) @@ -65,7 +52,7 @@ void opcontrol() { con.ButtonUp.pressed([]() { cata_sys.send_command(CataSys::Command::ToggleCata); }); pose_t start_pose = {.x = 16, .y = 144 - 16, .rot = 135}; - static std::atomic disable_drive(false); + con.ButtonA.pressed([]() { // Turn Right @@ -90,10 +77,26 @@ void opcontrol() { }); con.ButtonDown.pressed([]() { + printf("BUTTON DOQN PEWSSED\n"); // Climb - static bool isUp = false; - left_climb.set(isUp = !isUp); - right_climb.set(isUp); + CataOnlyState state = cata_sys.get_cata_state(); + switch (state) { + case CataOnlyState::Reloading: + case CataOnlyState::ReadyToFire: + case CataOnlyState::CataOff: + printf("start CLIMB\n"); + + cata_sys.send_command(CataSys::Command::StartClimb); + break; + case CataOnlyState::PrimeClimb: + cata_sys.send_command(CataSys::Command::FinishClimb); + printf("prme CLIMB\n"); + break; + case CataOnlyState::ClimbHold: + cata_sys.send_command(CataSys::Command::StopClimb); + printf("stop CLIMB\n"); + break; + } }); // personal debug button >:] @@ -102,10 +105,30 @@ void opcontrol() { // gps_localize_stdev(); }); - vision_light.set(false); - - static std::atomic brake_mode_toggled(false); con.ButtonX.pressed([]() { brake_mode_toggled = !brake_mode_toggled; }); +} + +/** + * Main entrypoint for the driver control period + */ +void opcontrol() { + // ================ TUNING CODE (Disable when not testing) ================ + testing(); + + // ================ INIT ================ + // Ensure the catapult system is enabled during driver control + if (cata_sys.get_cata_state() == CataOnlyState::CataOff) { + cata_sys.send_command(CataSys::Command::ToggleCata); + } + + static bool enable_matchload = false; + + left_wing.set(false); + right_wing.set(false); + + setupJoeControls(); + + vision_light.set(false); // ================ PERIODIC ================ while (true) { @@ -140,71 +163,57 @@ void opcontrol() { void testing() { // ================ AUTONOMOUS TESTING ================ - autonomous(); + // autonomous(); cata_sys.send_command(CataSys::Command::ToggleCata); + while (imu.isCalibrating() || gps_sensor.isCalibrating()) { vexDelay(20); } - // while(!con.ButtonA.pressing()) - // { - // tune_drive_ff_kv(TURN, 0.02); - // tune_drive_motion_maxv(TURN); - // tune_drive_motion_accel(TURN, 520); - // CommandController{ - // drive_sys.DriveToPointCmd({0, 12}, vex::forward), - // drive_sys.TurnToHeadingCmd(0), - // drive_sys.DriveToPointCmd({12, 12}, vex::forward), - // drive_sys.TurnToHeadingCmd(270), - // drive_sys.DriveToPointCmd({12, 0}, vex::forward), - // drive_sys.TurnToHeadingCmd(180), - // drive_sys.DriveToPointCmd({0, 0}, vex::forward), - // drive_sys.TurnToHeadingCmd(90), - // }.run(); - - // vexDelay(5); - // } - - static std::atomic disable_drive(false); + setupJoeControls(); // // ================ ODOMETRY TESTING ================ // // Reset encoder odometry to 0,0,90 with button X - con.ButtonX.pressed([]() { odom.set_position(); }); + // con.ButtonX.pressed([]() { odom.set_position(); }); // // Test localization with button Y - con.ButtonY.pressed([]() { - disable_drive = true; - vexDelay(500); // Settle first - GPSLocalizeCommand(RED).run(); - disable_drive = false; - }); + // con.ButtonY.pressed([]() { + // disable_drive = true; + // vexDelay(500); // Settle first + // GPSLocalizeCommand(RED).run(); + // disable_drive = false; + // }); // // ================ VISION TESTING ================ - con.ButtonRight.pressed([]() { vision_light.set(!vision_light.value()); }); + // con.ButtonRight.pressed([]() { vision_light.set(!vision_light.value()); }); - con.ButtonLeft.pressed([]() { cata_sys.send_command(CataSys::Command::ToggleCata); }); + // con.ButtonLeft.pressed([]() { cata_sys.send_command(CataSys::Command::ToggleCata); }); while (true) { // ================ Controls ================ double f = con.Axis3.position() / 100.0; double s = con.Axis1.position() / 100.0; + double l = con.Axis3.position() / 100.0; + double r = con.Axis2.position() / 100.0; + // if (!disable_drive) + // drive_sys.drive_tank(f, s, 1, TankDrive::BrakeType::None); if (!disable_drive) - drive_sys.drive_arcade(f, s, 1, TankDrive::BrakeType::None); + drive_sys.drive_tank(l, r, 1, TankDrive::BrakeType::None); // ================ Drive Tuning ================= static bool done_a = false; - if (con.ButtonA.pressing() && !done_a) { - disable_drive = true; - done_a = drive_sys.drive_to_point(24, 24, directionType::fwd, drive_mc_slow); - // done_a = drive_sys.drive_forward(24, vex::fwd); - // done_a = drive_sys.turn_degrees(100); - } else if (!con.ButtonA.pressing()) { - drive_sys.reset_auto(); - disable_drive = false; - done_a = false; - } + // if (con.ButtonA.pressing() && !done_a) { + // disable_drive = true; + // done_a = drive_sys.drive_to_point(24, 24, directionType::fwd, drive_mc_slow); + // // done_a = drive_sys.drive_forward(24, vex::fwd); + // // done_a = drive_sys.turn_degrees(100); + // } else if (!con.ButtonA.pressing()) { + // drive_sys.reset_auto(); + // disable_drive = false; + // done_a = false; + // } // ================ Debug Print Statements ================ pose_t odom_pose = odom.get_position(); @@ -214,19 +223,19 @@ void testing() { .rot = gps_sensor.heading() }; // printf("ODO: {%.2f, %.2f, %.2f} %f\n", odom_pose.x, odom_pose.y, odom_pose.rot, imu.heading(deg)); - printf("ODO: {%.2f, %.2f, %.2f} | GPS: {%.2f, %.2f, %.2f}\n", - odom_pose.x, odom_pose.y, odom_pose.rot, gps_pose.x, gps_pose.y, - gps_pose.rot); - - auto objs = vision_run_filter(TRIBALL); - int n = objs.size(), x = 0, y = 0, a = 0; - if (n > 0) { - x = objs[0].centerX; - y = objs[0].centerY; - a = objs[0].width * objs[0].height; - } - printf("CAM: N:%d | {%d, %d}, A:%d\n", n, x, y, a); - // printf("Pot: %f\n", cata_pot.angle(vex::deg)); + // printf("ODO: {%.2f, %.2f, %.2f} | GPS: {%.2f, %.2f, %.2f}\n", + // odom_pose.x, odom_pose.y, odom_pose.rot, gps_pose.x, gps_pose.y, + // gps_pose.rot); + + // auto objs = vision_run_filter(TRIBALL); + // int n = objs.size(), x = 0, y = 0, a = 0; + // if (n > 0) { + // x = objs[0].centerX; + // y = objs[0].centerY; + // a = objs[0].width * objs[0].height; + // } + // printf("CAM: N:%d | {%d, %d}, A:%d\n", n, x, y, a); + printf("Pot: %f\n", cata_pot.angle(vex::deg)); vexDelay(5); }