auto the next day with new gps and cjs toggle intake

include/automation.h

@@ -64,16 +64,17 @@ AutoCommand *WingSetCmd(bool val);
 
 AutoCommand *Climb();
 
-void gps_localize_median();
-std::tuple<pose_t, double> 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;
-};
+};

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<bool()> can_intake,
-              DropMode drop);
+              std::function<bool()> 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<bool()> can_intake;
+    std::function<bool()> ball_in_cata;
 };

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();

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<pose_t> gps_gather_data() {
     std::vector<pose_t> 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<pose_t, double> 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,13 +327,22 @@ std::tuple<pose_t, double> gps_localize_stdev() {
     return std::tuple<pose_t, double>(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;
 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)};
-}
+}

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<bool()> can_intake,
-                     DropMode drop)
+                     std::function<bool()> 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) {}

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 ||

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),

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