Add specifying angular direction to turns (#54)

* add specifying angular direction to turns

* Committing clang-format changes

* Fix a small little bug

---------

Co-authored-by: github-actions <41898282+github-actions[bot]@users.noreply.github.com>
Co-authored-by: Rocky14683 <[email protected]>

include/VOSS/chassis/AbstractChassis.hpp

@@ -24,7 +24,7 @@ class AbstractChassis {
                    double exitTime);
 
     void turn_task(controller_ptr controller, double max, voss::Flags flags,
-                   double exitTime);
+                   voss::AngularDirection direction, double exitTime);
 
   public:
     AbstractChassis(controller_ptr default_controller);
@@ -44,15 +44,23 @@ class AbstractChassis {
               voss::Flags flags = voss::Flags::NONE, double exitTime = 22500);
 
     void turn(double target, controller_ptr controller, double max = 100.0,
-              voss::Flags flags = voss::Flags::NONE, double exitTime = 22500);
+              voss::Flags flags = voss::Flags::NONE,
+              voss::AngularDirection direction = voss::AngularDirection::AUTO,
+              double exitTime = 22500);
     void turn(double target, double max = 100.0,
-              voss::Flags flags = voss::Flags::NONE, double exitTime = 22500);
-    void turn_to(Point target, controller_ptr controller, double max = 100.0,
-                 voss::Flags flags = voss::Flags::NONE,
-                 double exitTime = 22500);
-    void turn_to(Point target, double max = 100.0,
-                 voss::Flags flags = voss::Flags::NONE,
-                 double exitTime = 22500);
+              voss::Flags flags = voss::Flags::NONE,
+              voss::AngularDirection direction = voss::AngularDirection::AUTO,
+              double exitTime = 22500);
+    void
+    turn_to(Point target, controller_ptr controller, double max = 100.0,
+            voss::Flags flags = voss::Flags::NONE,
+            voss::AngularDirection direction = voss::AngularDirection::AUTO,
+            double exitTime = 22500);
+    void
+    turn_to(Point target, double max = 100.0,
+            voss::Flags flags = voss::Flags::NONE,
+            voss::AngularDirection direction = voss::AngularDirection::AUTO,
+            double exitTime = 22500);
 };
 
 } // namespace voss::chassis

include/VOSS/controller/AbstractController.hpp

@@ -2,6 +2,7 @@
 
 #include "VOSS/chassis/ChassisCommand.hpp"
 #include "VOSS/localizer/AbstractLocalizer.hpp"
+#include "VOSS/utils/flags.hpp"
 
 namespace voss::controller {
 
@@ -16,8 +17,9 @@ class AbstractController {
     AbstractController(std::shared_ptr<localizer::AbstractLocalizer> l);
 
     virtual chassis::ChassisCommand get_command(bool reverse, bool thru) = 0;
-    virtual chassis::ChassisCommand get_angular_command(bool reverse,
-                                                        bool thru) = 0;
+    virtual chassis::ChassisCommand
+    get_angular_command(bool reverse, bool thru,
+                        voss::AngularDirection direction) = 0;
 
     virtual void reset() = 0;
 

include/VOSS/controller/ArcPIDController.hpp

@@ -26,8 +26,9 @@ class ArcPIDController : public AbstractController {
     double linear_pid(double error);
 
     chassis::ChassisCommand get_command(bool reverse, bool thru) override;
-    chassis::ChassisCommand get_angular_command(bool reverse,
-                                                bool thru) override;
+    chassis::ChassisCommand
+    get_angular_command(bool reverse, bool thru,
+                        voss::AngularDirection direction) override;
 
     void reset() override;
 

include/VOSS/controller/BoomerangController.hpp

@@ -4,6 +4,7 @@
 #include "PIDController.hpp"
 #include "VOSS/chassis/ChassisCommand.hpp"
 #include "VOSS/localizer/AbstractLocalizer.hpp"
+#include "VOSS/utils/flags.hpp"
 
 namespace voss::controller {
 
@@ -16,8 +17,9 @@ class BoomerangController : public AbstractController {
     BoomerangController(std::shared_ptr<localizer::AbstractLocalizer> l);
 
     chassis::ChassisCommand get_command(bool reverse, bool thru) override;
-    chassis::ChassisCommand get_angular_command(bool reverse,
-                                                bool thru) override;
+    chassis::ChassisCommand
+    get_angular_command(bool reverse, bool thru,
+                        voss::AngularDirection direction) override;
 
     void reset();
 

include/VOSS/controller/PIDController.hpp

@@ -1,6 +1,7 @@
 #pragma once
 
 #include "VOSS/controller/AbstractController.hpp"
+#include "VOSS/utils/flags.hpp"
 
 namespace voss::controller {
 
@@ -19,6 +20,7 @@ class PIDController : public AbstractController {
     double close_2;
     int counter;
     double prev_angle;
+    bool turn_overshoot;
 
     double prev_lin_err, total_lin_err, prev_ang_err, total_ang_err;
 
@@ -29,8 +31,9 @@ class PIDController : public AbstractController {
     double angular_pid(double error);
 
     chassis::ChassisCommand get_command(bool reverse, bool thru) override;
-    chassis::ChassisCommand get_angular_command(bool reverse,
-                                                bool thru) override;
+    chassis::ChassisCommand
+    get_angular_command(bool reverse, bool thru,
+                        voss::AngularDirection direction) override;
 
     void reset() override;
 

include/VOSS/controller/SwingController.hpp

@@ -2,6 +2,7 @@
 
 #include "PIDController.hpp"
 #include "VOSS/controller/AbstractController.hpp"
+#include "VOSS/utils/flags.hpp"
 
 namespace voss::controller {
 class SwingController : public AbstractController {
@@ -14,6 +15,7 @@ class SwingController : public AbstractController {
     double close;
     double close_2;
     int counter;
+    bool turn_overshoot;
 
     double prev_ang_err, total_ang_err;
     double prev_ang_speed;
@@ -22,8 +24,9 @@ class SwingController : public AbstractController {
     SwingController(std::shared_ptr<localizer::AbstractLocalizer> l);
 
     chassis::ChassisCommand get_command(bool reverse, bool thru) override;
-    chassis::ChassisCommand get_angular_command(bool reverse,
-                                                bool thru) override;
+    chassis::ChassisCommand
+    get_angular_command(bool reverse, bool thru,
+                        voss::AngularDirection direction) override;
 
     double angular_pid(double error);
 

include/VOSS/utils/angle.hpp

@@ -1,13 +1,27 @@
 #pragma once
 
+#include "VOSS/utils/flags.hpp"
+#include <cmath>
+
 namespace voss {
 
-double to_radians(double degrees);
+inline double to_radians(double degrees) {
+    return degrees * M_PI / 180;
+}
 
-double to_degrees(double radians);
+inline double to_degrees(double radians) {
+    return radians * 180 * M_1_PI;
+}
 
-double norm(double radians);
+inline double norm(double radians) {
+    radians = fmod(radians, 2 * M_PI);
+    if (radians < 0)
+        radians += 2 * M_PI;
+    return radians;
+}
 
-double norm_delta(double radians);
+inline double norm_delta(double radians) {
+    return std::remainder(radians, 2 * M_PI);
+}
 
 } // namespace voss

include/VOSS/utils/flags.hpp

@@ -21,4 +21,6 @@ auto inline operator&(Flags flag1, Flags flag2) {
     return static_cast<bool>(static_cast<uint8_t>(flag1) &
                              static_cast<uint8_t>(flag2));
 }
+
+enum class AngularDirection { AUTO, COUNTERCLOCKWISE, CLOCKWISE };
 } // namespace voss

src/VOSS/chassis/AbstractChassis.cpp

@@ -41,14 +41,16 @@ void AbstractChassis::move_task(controller_ptr controller, double max,
 }
 
 void AbstractChassis::turn_task(controller_ptr controller, double max,
-                                voss::Flags flags, double exitTime) {
+                                voss::Flags flags,
+                                voss::AngularDirection direction,
+                                double exitTime) {
     int t = 0;
     pros::Task running_t([&, controller]() {
         controller->reset();
-        while (!this->execute(
-            controller->get_angular_command(flags & voss::Flags::REVERSE,
-                                            flags & voss::Flags::THRU),
-            max)) {
+        while (!this->execute(controller->get_angular_command(
+                                  flags & voss::Flags::REVERSE,
+                                  flags & voss::Flags::THRU, direction),
+                              max)) {
             if (pros::competition::is_disabled()) {
                 return;
             }
@@ -95,33 +97,39 @@ void AbstractChassis::move(Pose target, controller_ptr controller, double max,
 }
 
 void AbstractChassis::turn(double target, double max, voss::Flags flags,
-                           double exitTime) {
-    this->turn(target, this->default_controller, max, flags, exitTime);
+                           voss::AngularDirection direction, double exitTime) {
+    this->turn(target, this->default_controller, max, flags, direction,
+               exitTime);
 }
 
 void AbstractChassis::turn(double target, controller_ptr controller, double max,
-                           voss::Flags flags, double exitTime) {
+                           voss::Flags flags, voss::AngularDirection direction,
+                           double exitTime) {
     // this->m.take();
 
     controller->set_target({0, 0, 0}, false);
     controller->set_angular_target(target, flags & voss::Flags::RELATIVE);
 
-    this->turn_task(controller, max, flags, exitTime);
+    this->turn_task(controller, max, flags, direction, exitTime);
 }
 
 void AbstractChassis::turn_to(Point target, double max, voss::Flags flags,
+                              voss::AngularDirection direction,
                               double exitTime) {
-    this->turn_to(target, this->default_controller, max, flags, exitTime);
+    this->turn_to(target, this->default_controller, max, flags, direction,
+                  exitTime);
 }
 
 void AbstractChassis::turn_to(Point target, controller_ptr controller,
-                              double max, voss::Flags flags, double exitTime) {
+                              double max, voss::Flags flags,
+                              voss::AngularDirection direction,
+                              double exitTime) {
     // this->m.take();
 
     controller->set_target({target.x, target.y, 361},
                            flags & voss::Flags::RELATIVE);
 
-    this->turn_task(controller, max, flags, exitTime);
+    this->turn_task(controller, max, flags, direction, exitTime);
 }
 
 } // namespace voss::chassis

src/VOSS/controller/ArcPIDController.cpp

@@ -88,8 +88,9 @@ chassis::ChassisCommand ArcPIDController::get_command(bool reverse, bool thru) {
     return chassis::ChassisCommand{chassis::Voltages{left_speed, right_speed}};
 }
 
-chassis::ChassisCommand ArcPIDController::get_angular_command(bool reverse,
-                                                              bool thru) {
+chassis::ChassisCommand
+ArcPIDController::get_angular_command(bool reverse, bool thru,
+                                      voss::AngularDirection direction) {
     return chassis::ChassisCommand{chassis::Stop{}};
 }
 

src/VOSS/controller/BoomerangController.cpp

@@ -32,11 +32,12 @@ chassis::ChassisCommand BoomerangController::get_command(bool reverse,
     return child->get_command(reverse, thru);
 }
 
-chassis::ChassisCommand BoomerangController::get_angular_command(bool reverse,
-                                                                 bool thru) {
+chassis::ChassisCommand
+BoomerangController::get_angular_command(bool reverse, bool thru,
+                                         voss::AngularDirection direction) {
     child->set_target(target, false);
     child->set_angular_target(angular_target, false);
-    return child->get_angular_command(reverse, thru);
+    return child->get_angular_command(reverse, thru, direction);
 }
 
 void BoomerangController::reset() {

src/VOSS/controller/PIDController.cpp

@@ -104,8 +104,9 @@ chassis::ChassisCommand PIDController::get_command(bool reverse, bool thru) {
         chassis::Voltages{lin_speed - ang_speed, lin_speed + ang_speed}};
 }
 
-chassis::ChassisCommand PIDController::get_angular_command(bool reverse,
-                                                           bool thru) {
+chassis::ChassisCommand
+PIDController::get_angular_command(bool reverse, bool thru,
+                                   voss::AngularDirection direction) {
     counter += 10;
     double current_angle = this->l->get_orientation_rad();
     double target_angle = 0;
@@ -121,6 +122,20 @@ chassis::ChassisCommand PIDController::get_angular_command(bool reverse,
 
     angular_error = voss::norm_delta(angular_error);
 
+    if (fabs(angular_error) < voss::to_radians(5)) {
+        turn_overshoot = true;
+    }
+
+    if (!turn_overshoot) {
+        if (direction == voss::AngularDirection::COUNTERCLOCKWISE &&
+            angular_error < 0) {
+            angular_error += 2 * M_PI;
+        } else if (direction == voss::AngularDirection::CLOCKWISE &&
+                   angular_error > 0) {
+            angular_error -= 2 * M_PI;
+        }
+    }
+
     if (fabs(angular_error) < angular_exit_error) {
         close += 10;
     } else {
@@ -173,6 +188,7 @@ void PIDController::reset() {
     this->total_ang_err = 0;
     this->can_reverse = false;
     this->counter = 0;
+    this->turn_overshoot = false;
 }
 
 } // namespace voss::controller

src/VOSS/controller/SwingController.cpp

@@ -10,8 +10,9 @@ chassis::ChassisCommand SwingController::get_command(bool reverse, bool thru) {
     return chassis::ChassisCommand{chassis::Stop{}};
 }
 
-chassis::ChassisCommand SwingController::get_angular_command(bool reverse,
-                                                             bool thru) {
+chassis::ChassisCommand
+SwingController::get_angular_command(bool reverse, bool thru,
+                                     voss::AngularDirection direction) {
     counter += 10;
     double current_angle = this->l->get_orientation_rad();
     double target_angle = 0;
@@ -27,6 +28,20 @@ chassis::ChassisCommand SwingController::get_angular_command(bool reverse,
 
     angular_error = voss::norm_delta(angular_error);
 
+    if (fabs(angular_error) < voss::to_radians(5)) {
+        turn_overshoot = true;
+    }
+
+    if (!turn_overshoot) {
+        if (direction == voss::AngularDirection::COUNTERCLOCKWISE &&
+            angular_error < 0) {
+            angular_error += 2 * M_PI;
+        } else if (direction == voss::AngularDirection::CLOCKWISE &&
+                   angular_error > 0) {
+            angular_error -= 2 * M_PI;
+        }
+    }
+
     if (fabs(angular_error) < angular_exit_error) {
         close += 10;
     } else {
@@ -51,7 +66,7 @@ chassis::ChassisCommand SwingController::get_angular_command(bool reverse,
     chassis::ChassisCommand command;
     if (!((ang_speed >= 0.0) ^ (this->prev_ang_speed < 0.0)) &&
         this->prev_ang_speed != 0) {
-        can_reverse = true;
+        can_reverse = !can_reverse;
     }
 
     if (!this->can_reverse) {
@@ -94,5 +109,6 @@ void SwingController::reset() {
     this->total_ang_err = 0;
     this->counter = 0;
     this->can_reverse = false;
+    this->turn_overshoot = false;
 }
 }; // namespace voss::controller