Spring 2025 Improvements

.github/workflows/clang-tidy.yml

@@ -25,4 +25,4 @@ jobs:
         -   name: Build Project
             run: pros build-compile-commands
         -   name: Run clang-tidy
-            uses: ZedThree/clang-tidy-review@v0.14.0
+            uses: ZedThree/clang-tidy-review@v0.20.1

Makefile

@@ -27,7 +27,7 @@ EXCLUDE_COLD_LIBRARIES:=
 IS_LIBRARY:=1
 # TODO: CHANGE THIS!
 LIBNAME:=VOSS
-VERSION:=0.1.4
+VERSION:=0.1.5
 # EXCLUDE_SRC_FROM_LIB= $(SRCDIR)/unpublishedfile.c
 # this line excludes opcontrol.c and similar files
 EXCLUDE_SRC_FROM_LIB+=$(foreach file, $(SRCDIR)/main,$(foreach cext,$(CEXTS),$(file).$(cext)) $(foreach cxxext,$(CXXEXTS),$(file).$(cxxext)))

include/VOSS/api.hpp

@@ -35,6 +35,7 @@
 #include "selector/Selector.hpp"
 
 #include "utils/angle.hpp"
+#include "utils/debug.hpp"
 #include "utils/flags.hpp"
 #include "utils/Point.hpp"
 #include "utils/Pose.hpp"

include/VOSS/chassis/AbstractChassis.hpp

@@ -39,6 +39,9 @@ class AbstractChassis {
     virtual bool execute(DiffChassisCommand cmd, double max) = 0;
     virtual void set_brake_mode(pros::motor_brake_mode_e mode) = 0;
 
+    void wait_until_done() const;
+    bool is_running() const;
+
     void move(double distance, double max = 100.0,
               voss::Flags flags = voss::Flags::NONE);
 

include/VOSS/controller/BoomerangController.hpp

@@ -11,17 +11,34 @@
 namespace voss::controller {
 
 class BoomerangController : public AbstractController {
+  public:
+    enum class ThruBehavior { // affects calculation of lin_speed on thru movements
+      MIN_VEL, // use linear PID, but enforce a minimum velocity
+      FULL_SPEED // no PID, go full speed
+    };
+    enum class CosineScaling {
+      MIN_ERR, // only do cosine scaling within the minimum err
+      ALL_THE_TIME // do cosine scaling all the time
+    };
+    enum class MinErrBehavior {
+      TARGET_HEADING, // directly switch from turning towards carrot point to turning towards target heading
+      SCALE_TARGET_HEADING, // scale between turning towards carrot point to turning towards target point
+    };
   protected:
     std::shared_ptr<BoomerangController> p;
     double lead_pct;
-    Pose carrotPoint;
 
     utils::PID linear_pid, angular_pid;
     double min_error;
     bool can_reverse;
 
     double min_vel;
 
+    ThruBehavior thru_behavior = ThruBehavior::FULL_SPEED;
+    CosineScaling cosine_scaling = CosineScaling::ALL_THE_TIME;
+    MinErrBehavior min_err_behavior = MinErrBehavior::SCALE_TARGET_HEADING;
+    bool enable_overturn = false;
+
   public:
     BoomerangController(std::shared_ptr<localizer::AbstractLocalizer> l);
 
@@ -42,6 +59,11 @@ class BoomerangController : public AbstractController {
     std::shared_ptr<BoomerangController> modify_min_error(double error);
     std::shared_ptr<BoomerangController> modify_lead_pct(double lead_pct);
 
+    void set_thru_behavior(ThruBehavior thru_behavior);
+    void set_cosine_scaling(CosineScaling cosine_scaling);
+    void set_min_err_behavior(MinErrBehavior min_err_behavior);
+    void set_overturn(bool overturn);
+
     friend class BoomerangControllerBuilder;
 };
 

include/VOSS/controller/PIDController.hpp

@@ -7,16 +7,36 @@
 namespace voss::controller {
 
 class PIDController : public AbstractController {
+  public:
+    enum class ThruBehavior { // affects calculation of lin_speed on thru movements
+      MIN_VEL, // use linear PID, but enforce a minimum velocity
+      FULL_SPEED // no PID, go full speed
+    };
+    enum class CosineScaling {
+      MIN_ERR, // only do cosine scaling within the minimum err
+      ALL_THE_TIME // do cosine scaling all the time
+    };
+    enum class MinErrBehavior {
+      NO_ANG_PID, // no angular PID, set ang_speed to 0
+      SCALE_ANG_PID, // scale output of angular pid so it reaches 0 at half of min_err
+      MAINTAIN_HEADING // maintain heading of when it entered min_err
+    };
   protected:
     std::shared_ptr<PIDController> p;
 
     utils::PID linear_pid, angular_pid;
     double min_error;
     bool can_reverse;
+    double min_err_heading;
 
     double min_vel;
     bool turn_overshoot;
 
+    ThruBehavior thru_behavior = ThruBehavior::FULL_SPEED;
+    CosineScaling cosine_scaling = CosineScaling::MIN_ERR;
+    MinErrBehavior min_err_behavior = MinErrBehavior::MAINTAIN_HEADING;
+    bool enable_overturn = false;
+
   public:
     PIDController(std::shared_ptr<localizer::AbstractLocalizer> l);
 
@@ -36,6 +56,11 @@ class PIDController : public AbstractController {
     modify_angular_constants(double kP, double kI, double kD);
     std::shared_ptr<PIDController> modify_min_error(double min_error);
 
+    void set_thru_behavior(ThruBehavior thru_behavior);
+    void set_cosine_scaling(CosineScaling cosine_scaling);
+    void set_min_err_behavior(MinErrBehavior min_err_behavior);
+    void set_overturn(bool overturn);
+
     friend class PIDControllerBuilder;
     friend class BoomerangControllerBuilder;
 };

include/VOSS/localizer/AbstractLocalizer.hpp

@@ -27,6 +27,8 @@ class AbstractLocalizer {
     double get_orientation_rad();
     double get_orientation_deg();
     Point get_position();
+    void wait_until_near(Point target, double tolerance);
+    void wait_until_distance(double distance);
     virtual void calibrate() = 0;
 };
 

include/VOSS/localizer/TrackingWheelLocalizer.hpp

@@ -12,6 +12,8 @@ class TrackingWheelLocalizer : public AbstractLocalizer {
   protected:
     std::atomic<double> prev_left_pos, prev_right_pos, prev_middle_pos;
     AtomicPose prev_pose;
+    AtomicPose real_pose;
+    Pose local_offset = {0, 0, 0};
 
     std::atomic<double> left_right_dist, middle_dist;
     std::unique_ptr<AbstractTrackingWheel> left_tracking_wheel,
@@ -28,6 +30,7 @@ class TrackingWheelLocalizer : public AbstractLocalizer {
     void calibrate() override;
     void set_pose(Pose pose) override;
     void set_pose(double x, double y, double theta) override;
+    void set_local_offset(Pose local_offset);
 
     friend class TrackingWheelLocalizerBuilder;
 };

include/VOSS/utils/debug.hpp

@@ -0,0 +1,9 @@
+#pragma once
+
+namespace voss {
+
+void enable_debug();
+void disable_debug();
+bool get_debug();
+
+}

src/VOSS/chassis/AbstractChassis.cpp

@@ -13,6 +13,16 @@ AbstractChassis::AbstractChassis(controller_ptr default_controller, ec_ptr ec) {
     this->default_ec = std::move(ec);
 }
 
+void AbstractChassis::wait_until_done() const {
+    while (task_running) {
+        pros::delay(constants::MOTOR_UPDATE_DELAY);
+    }
+}
+
+bool AbstractChassis::is_running() const {
+    return task_running;
+}
+
 void AbstractChassis::move_task(controller_ptr controller, ec_ptr ec,
                                 double max, voss::Flags flags) {
 
@@ -154,7 +164,7 @@ void AbstractChassis::turn_to(Point target, controller_ptr controller,
                               ec_ptr ec, double max, voss::Flags flags,
                               voss::AngularDirection direction) {
     while (this->task_running) {
-        pros::delay(10);
+        pros::delay(constants::MOTOR_UPDATE_DELAY);
     }
     this->task_running = true;
 

src/VOSS/controller/BoomerangController.cpp

@@ -3,6 +3,7 @@
 #include "PIDController.hpp"
 #include "VOSS/chassis/ChassisCommand.hpp"
 #include "VOSS/utils/angle.hpp"
+#include "VOSS/utils/debug.hpp"
 #include <cmath>
 
 namespace voss::controller {
@@ -23,18 +24,23 @@ BoomerangController::get_command(bool reverse, bool thru,
     Point current_pos = this->l->get_position();
 
     int dir = reverse ? -1 : 1;
-    Pose trueTarget;
     double dx = target.x - current_pos.x;
     double dy = target.y - current_pos.y;
 
     double distance_error = sqrt(dx * dx + dy * dy);
-    double at = target.theta.value();
+    double target_angle = target.theta.value();
 
-    this->carrotPoint = {this->target.x - distance_error * cos(at) * lead_pct,
-                         this->target.y - distance_error * sin(at) * lead_pct,
-                         target.theta};
-    dx = carrotPoint.x - current_pos.x;
-    dy = carrotPoint.y - current_pos.y;
+    Point carrot_point;
+    if (!reverse) {
+        carrot_point = {target.x - distance_error * cos(target_angle) * lead_pct,
+                        target.y - distance_error * sin(target_angle) * lead_pct};
+    } else {
+        carrot_point = {target.x + distance_error * cos(target_angle) * lead_pct,
+                        target.y + distance_error * sin(target_angle) * lead_pct};
+    }
+
+    dx = carrot_point.x - current_pos.x;
+    dy = carrot_point.y - current_pos.y;
     double current_angle = this->l->get_orientation_rad();
 
     double angle_error;
@@ -48,42 +54,57 @@ BoomerangController::get_command(bool reverse, bool thru,
 
     double lin_speed = linear_pid.update(distance_error);
     if (thru) {
-        lin_speed = copysign(fmax(fabs(lin_speed), this->min_vel), lin_speed);
+        if (thru_behavior == ThruBehavior::FULL_SPEED) {
+            lin_speed = 100;
+        } else {
+            lin_speed = fmax(lin_speed, min_vel);
+        }
+    }
+    if (cosine_scaling == CosineScaling::ALL_THE_TIME || distance_error < min_error) {
+        lin_speed *= cos(angle_error);
     }
     lin_speed *= dir;
 
-    double pose_error = voss::norm_delta(target.theta.value() - current_angle);
-
     double ang_speed;
     if (distance_error < min_error) {
         this->can_reverse = true;
+        double pose_error = voss::norm_delta(target_angle - current_angle);
 
-        ang_speed = angular_pid.update(pose_error);
-    } else if (distance_error < 2 * min_error) {
-        double scale_factor = (distance_error - min_error) / min_error;
-        double scaled_angle_error = voss::norm_delta(
-            scale_factor * angle_error + (1 - scale_factor) * pose_error);
-
-        ang_speed = angular_pid.update(scaled_angle_error);
+        if (min_err_behavior == MinErrBehavior::TARGET_HEADING) {
+            ang_speed = angular_pid.update(pose_error);
+        } else {
+            double min_err_2 = min_error / 2;
+            double scale_factor = (distance_error - min_err_2) / min_err_2;
+            scale_factor = fmax(0.0, scale_factor);
+            double scaled_angle_error = norm_delta(scale_factor * angle_error + (1 - scale_factor) * pose_error);
+            ang_speed = angular_pid.update(scaled_angle_error);
+        }
     } else {
         if (fabs(angle_error) > M_PI_2 && this->can_reverse) {
             angle_error =
                 angle_error - (angle_error / fabs(angle_error)) * M_PI;
-            lin_speed = -lin_speed;
+            if (cosine_scaling != CosineScaling::ALL_THE_TIME) {
+                lin_speed = -lin_speed;
+            }
         }
 
         ang_speed = angular_pid.update(angle_error);
     }
 
-    lin_speed *= cos(angle_error);
+    lin_speed = std::clamp(lin_speed, -100.0, 100.0);
+    if (enable_overturn) {
+        ang_speed = std::clamp(ang_speed, -100.0, 100.0);
+        lin_speed = std::clamp(lin_speed, -100 + fabs(ang_speed), 100 - fabs(ang_speed));
+    }
 
-    lin_speed = std::max(-100.0, std::min(100.0, lin_speed));
+    if (get_debug()) {
+        printf("carrot point: %.2f, %.2f, dist_err: %.2f, ang_err: %.2f, lin_speed: %.2f, ang_speed: %.2f\n", carrot_point.x, carrot_point.y, angle_error, lin_speed, ang_speed);
+    }
 
     if (ec->is_met(this->l->get_pose(), thru)) {
         if (thru) {
             return chassis::DiffChassisCommand{chassis::diff_commands::Chained{
-                dir * std::fmax(lin_speed, this->min_vel) - ang_speed,
-                dir * std::fmax(lin_speed, this->min_vel) + ang_speed}};
+                lin_speed - ang_speed, lin_speed + ang_speed}};
         } else {
             return chassis::Stop{};
         }
@@ -148,4 +169,20 @@ BoomerangController::modify_lead_pct(double lead_pct) {
     return this->p;
 }
 
+void BoomerangController::set_thru_behavior(ThruBehavior thru_behavior) {
+    this->thru_behavior = thru_behavior;
+}
+
+void BoomerangController::set_cosine_scaling(CosineScaling cosine_scaling) {
+    this->cosine_scaling = cosine_scaling;
+}
+
+void BoomerangController::set_min_err_behavior(MinErrBehavior min_err_behavior) {
+    this->min_err_behavior = min_err_behavior;
+}
+
+void BoomerangController::set_overturn(bool overturn) {
+    enable_overturn = overturn;
+}
+
 } // namespace voss::controller