diff --git a/include/automation.h b/include/automation.h index 0125d19..150ec29 100644 --- a/include/automation.h +++ b/include/automation.h @@ -64,16 +64,17 @@ AutoCommand *WingSetCmd(bool val); AutoCommand *Climb(); -void gps_localize_median(); -std::tuple gps_localize_stdev(); +enum FieldSide { RED, BLUE }; class GPSLocalizeCommand : public AutoCommand { public: + GPSLocalizeCommand(FieldSide s); bool run() override; static pose_t get_pose_rotated(); private: + FieldSide side; static bool first_run; static int rotation; static const int min_rotation_radius; -}; \ No newline at end of file +}; diff --git a/include/cata/intake.h b/include/cata/intake.h index 0698fb8..a0cadc7 100644 --- a/include/cata/intake.h +++ b/include/cata/intake.h @@ -35,7 +35,7 @@ class IntakeSys IntakeSys(vex::distance &intake_watcher, vex::motor &intake_lower, vex::motor &intake_upper, std::function can_intake, - DropMode drop); + std::function ball_in_cata, DropMode drop); bool ball_in_intake(); @@ -44,4 +44,5 @@ class IntakeSys vex::motor &intake_lower; vex::motor &intake_upper; std::function can_intake; + std::function ball_in_cata; }; diff --git a/include/cata_system.h b/include/cata_system.h index e6fa1c1..12508d8 100644 --- a/include/cata_system.h +++ b/include/cata_system.h @@ -29,6 +29,7 @@ class CataSys { // Returns true when the cata system is finished dropping bool still_dropping(); bool ball_in_intake(); + bool intake_running(); // Autocommands AutoCommand *Fire(); diff --git a/src/automation.cpp b/src/automation.cpp index d5c656c..668005e 100644 --- a/src/automation.cpp +++ b/src/automation.cpp @@ -230,12 +230,13 @@ AutoCommand *Climb() { // ================ GPS Localizing Functions ================ #define NUM_DATAPOINTS 100 -#define GPS_GATHER_SEC 0.75 +#define GPS_GATHER_SEC 1.0 std::vector gps_gather_data() { std::vector pose_list; vex::timer tmr; + // for(int i = 0; i < NUM_DATAPOINTS; i++) while (tmr.time(sec) < GPS_GATHER_SEC) { pose_t cur; cur.x = gps_sensor.xPosition(distanceUnits::in) + 72; @@ -316,7 +317,7 @@ std::tuple gps_localize_stdev() { pose_list.erase(itr, pose_list.end()); pose_t avg_filtered = get_pose_avg(pose_list); - printf("Stddev: %f #Unfiltered: %lu #Filtered: %lu\n", dist_stdev, + printf("Stddev: %f #Unfiltered: %d #Filtered: %d\n", dist_stdev, dist_list.size(), pose_list.size()); printf("Unfiltered X: %f, Y: %f, H: %f\n", avg_unfiltered.x, avg_unfiltered.y, avg_unfiltered.rot); @@ -326,6 +327,8 @@ std::tuple gps_localize_stdev() { return std::tuple(avg_filtered, dist_stdev); } +GPSLocalizeCommand::GPSLocalizeCommand(FieldSide s) : side(s) {} + bool GPSLocalizeCommand::first_run = true; int GPSLocalizeCommand::rotation = 0; const int GPSLocalizeCommand::min_rotation_radius = 48; @@ -333,6 +336,13 @@ bool GPSLocalizeCommand::run() { // pose_t odom_pose = odom.get_position(); vexDelay(500); // Let GPS settle auto [new_pose, stddev] = gps_localize_stdev(); + + if (side == BLUE) { + new_pose.x = 144 - new_pose.x; + new_pose.y = 144 - new_pose.y; + new_pose.rot = new_pose.rot + 180; + } + odom.set_position(new_pose); printf("Localized with max variation of %f to {%f, %f, %f}\n", stddev, @@ -350,4 +360,4 @@ pose_t GPSLocalizeCommand::get_pose_rotated() { return pose_t{.x = rot.get_x(), .y = rot.get_y(), .rot = gps_sensor.heading(rotationUnits::deg)}; -} \ No newline at end of file +} diff --git a/src/cata/intake.cpp b/src/cata/intake.cpp index e21f144..981e7d7 100644 --- a/src/cata/intake.cpp +++ b/src/cata/intake.cpp @@ -91,6 +91,13 @@ struct Intaking : IntakeSys::State { sys.intake_upper.spin(vex::fwd, intake_upper_volt, vex::volt); sys.intake_lower.spin(vex::fwd, intake_lower_volt, vex::volt); } + IntakeSys::MaybeMessage work(IntakeSys &sys) override { + if (sys.ball_in_cata()) { + return IntakeMessage::StopIntake; + } + + return {}; + } void exit(IntakeSys &sys) override { sys.intake_upper.stop(vex::brakeType::coast); sys.intake_lower.stop(vex::brakeType::coast); @@ -195,9 +202,10 @@ IntakeSys::State *Outtaking::respond(IntakeSys &sys, IntakeMessage m) { IntakeSys::IntakeSys(vex::distance &intake_watcher, vex::motor &intake_lower, vex::motor &intake_upper, std::function can_intake, - DropMode drop) + std::function ball_in_cata, DropMode drop) : StateMachine(drop == DropMode::Required ? (IntakeSys::State *)(new IntakeWaitForDrop()) : (IntakeSys::State *)(new Stopped())), intake_watcher(intake_watcher), intake_lower(intake_lower), - intake_upper(intake_upper), can_intake(can_intake) {} + intake_upper(intake_upper), can_intake(can_intake), + ball_in_cata(ball_in_cata) {} diff --git a/src/cata_system.cpp b/src/cata_system.cpp index d82f1ed..48347ed 100644 --- a/src/cata_system.cpp +++ b/src/cata_system.cpp @@ -11,7 +11,8 @@ CataSys::CataSys(vex::distance &intake_watcher, vex::pot &cata_pot, cata_sys(cata_pot, cata_watcher, cata_motor, cata_feedback, drop), intake_sys( intake_watcher, intake_lower, intake_upper, - [&]() { return cata_sys.intaking_allowed(); }, drop) {} + [&]() { return cata_sys.intaking_allowed(); }, + [&]() { return cata_watcher.isNearObject(); }, drop) {} void CataSys::send_command(Command next_cmd) { switch (next_cmd) { @@ -53,6 +54,11 @@ void CataSys::send_command(Command next_cmd) { break; } } + +bool CataSys::intake_running() { + return !(intake_sys.current_state() == IntakeState::Stopped); +} + bool CataSys::still_dropping() { bool still_dropping = cata_sys.current_state() == CataOnlyState::WaitingForDrop || diff --git a/src/competition/autonomous.cpp b/src/competition/autonomous.cpp index 5cdda36..fd48c06 100644 --- a/src/competition/autonomous.cpp +++ b/src/competition/autonomous.cpp @@ -140,6 +140,11 @@ AutoCommand *get_and_score_alliance() { void supportMaximumTriballs() { + FieldSide fs = FieldSide::BLUE; + if (red_side) { + fs = FieldSide::RED; + } + odom.set_position({.x = 28, .y = 18, .rot = 180}); CommandController cc{ @@ -192,9 +197,9 @@ void supportMaximumTriballs() { cata_sys.StopIntake(), }, - new IfTimePassed(41)), + new IfTimePassed(31)), drive_sys.TurnToPointCmd(0, 0)->withTimeout(2.0), - new GPSLocalizeCommand(), + new GPSLocalizeCommand(fs), cata_sys.IntakeToHold(), drive_sys.DriveToPointCmd({0, 0}, FWD, 0.3)->withTimeout(2.0), drive_sys.DriveForwardCmd(4, REV)->withTimeout(2.0), diff --git a/src/competition/opcontrol.cpp b/src/competition/opcontrol.cpp index bd43e8d..61ca168 100644 --- a/src/competition/opcontrol.cpp +++ b/src/competition/opcontrol.cpp @@ -42,10 +42,6 @@ void opcontrol() { con.ButtonL1.pressed( []() { cata_sys.send_command(CataSys::Command::StartFiring); }); - con.ButtonR1.pressed( - []() { cata_sys.send_command(CataSys::Command::IntakeIn); }); - con.ButtonR2.pressed( - []() { cata_sys.send_command(CataSys::Command::IntakeOut); }); con.ButtonY.pressed([]() { Tank = !Tank; }); con.ButtonUp.pressed( @@ -55,15 +51,17 @@ void opcontrol() { right_wing.set(!right_wing.value()); }); + con.ButtonR1.pressed( + []() { cata_sys.send_command(CataSys::Command::IntakeIn); }); + con.ButtonR2.pressed( + []() { cata_sys.send_command(CataSys::Command::IntakeOut); }); + con.ButtonRight.pressed( + []() { cata_sys.send_command(CataSys::Command::StopIntake); }); + #endif con.ButtonB.pressed([]() { toggle_brake_mode(); }); // ================ INIT ================ while (true) { - if (!con.ButtonR1.pressing() && !con.ButtonR2.pressing()) { - if (!cata_sys.still_dropping()) { - cata_sys.send_command(CataSys::Command::StopIntake); - } - } if (Tank) { double l = con.Axis3.position() / 100.0; @@ -82,22 +80,6 @@ void opcontrol() { } static VisionTrackTriballCommand viscmd; - if (con.ButtonX.pressing()) { - disable_drive = true; - bool override_watch = false; -#ifndef COMP_BOT - override_watch = true; -#endif - if (override_watch || - intake_watcher.objectDistance(distanceUnits::mm) > 100.0) { - viscmd.run(); - } else { - drive_sys.stop(); - cata_sys.send_command(CataSys::Command::StopIntake); - } - } else { - disable_drive = false; - } // Controls // Intake