Spring 2025 Improvements (#115)

* improvements

* print carrot point in boomerang debug

* update clang-tidy action

* bump version number

Author: AndrewLuGit

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