From 1bb7f5e02f85d244908a85b42659af83450afc47 Mon Sep 17 00:00:00 2001
From: THERocky <101498190+Rocky14683@users.noreply.github.com>
Date: Tue, 19 Mar 2024 19:52:16 -0400
Subject: [PATCH] Relative move (#71)

---------

Co-authored-by: github-actions <41898282+github-actions[bot]@users.noreply.github.com>
---
 include/VOSS/chassis/AbstractChassis.hpp    |  9 +++++++++
 include/VOSS/utils/flags.hpp                |  8 +++++++-
 src/VOSS/chassis/AbstractChassis.cpp        | 17 +++++++++++++++++
 src/VOSS/chassis/DiffChassis.cpp            |  2 ++
 src/VOSS/controller/BoomerangController.cpp |  1 +
 src/VOSS/controller/PIDController.cpp       |  1 +
 src/VOSS/localizer/AbstractLocalizer.cpp    |  1 -
 7 files changed, 37 insertions(+), 2 deletions(-)

diff --git a/include/VOSS/chassis/AbstractChassis.hpp b/include/VOSS/chassis/AbstractChassis.hpp
index ba26810..22c4381 100644
--- a/include/VOSS/chassis/AbstractChassis.hpp
+++ b/include/VOSS/chassis/AbstractChassis.hpp
@@ -39,6 +39,15 @@ class AbstractChassis {
     virtual bool execute(DiffChassisCommand cmd, double max) = 0;
     virtual void set_brake_mode(pros::motor_brake_mode_e mode) = 0;
 
+    void move(double distance, double max = 100.0,
+              voss::Flags flags = voss::Flags::NONE);
+
+    void move(double distance, controller_ptr controller, double max = 100.0,
+              voss::Flags flags = voss::Flags::NONE);
+
+    void move(double distance, controller_ptr controller, ec_ptr ec,
+              double max = 100.0, voss::Flags flags = voss::Flags::NONE);
+
     void move(Pose target, controller_ptr controller, ec_ptr ec,
               double max = 100.0, voss::Flags flags = voss::Flags::NONE);
 
diff --git a/include/VOSS/utils/flags.hpp b/include/VOSS/utils/flags.hpp
index 559ba90..683a347 100644
--- a/include/VOSS/utils/flags.hpp
+++ b/include/VOSS/utils/flags.hpp
@@ -22,5 +22,11 @@ auto inline operator&(Flags flag1, Flags flag2) {
                              static_cast<uint8_t>(flag2));
 }
 
-enum class AngularDirection { AUTO, COUNTERCLOCKWISE, CLOCKWISE };
+enum class AngularDirection {
+    AUTO,
+    COUNTERCLOCKWISE,
+    CCW = COUNTERCLOCKWISE,
+    CLOCKWISE,
+    CW = CLOCKWISE
+};
 } // namespace voss
\ No newline at end of file
diff --git a/src/VOSS/chassis/AbstractChassis.cpp b/src/VOSS/chassis/AbstractChassis.cpp
index e6d6c75..9665837 100644
--- a/src/VOSS/chassis/AbstractChassis.cpp
+++ b/src/VOSS/chassis/AbstractChassis.cpp
@@ -70,6 +70,23 @@ void AbstractChassis::turn_task(controller_ptr controller, ec_ptr ec,
     this->task->join();
 }
 
+void AbstractChassis::move(double distance, double max, voss::Flags flags) {
+    this->move({distance, 0}, this->default_controller, this->default_ec, max,
+               flags | voss::Flags::RELATIVE);
+}
+
+void AbstractChassis::move(double distance, controller_ptr controller,
+                           double max, voss::Flags flags) {
+    this->move({distance, 0}, std::move(controller), this->default_ec, max,
+               flags | voss::Flags::RELATIVE);
+}
+
+void AbstractChassis::move(double distance, controller_ptr controller,
+                           ec_ptr ec, double max, voss::Flags flags) {
+    this->move({distance, 0}, std::move(controller), std::move(ec), max,
+               flags | Flags::RELATIVE);
+}
+
 void AbstractChassis::move(Pose target, double max, voss::Flags flags) {
     this->move(target, this->default_controller, this->default_ec, max, flags);
 }
diff --git a/src/VOSS/chassis/DiffChassis.cpp b/src/VOSS/chassis/DiffChassis.cpp
index 38f73e9..ba7a105 100644
--- a/src/VOSS/chassis/DiffChassis.cpp
+++ b/src/VOSS/chassis/DiffChassis.cpp
@@ -35,6 +35,8 @@ DiffChassis::DiffChassis(std::initializer_list<int8_t> left_motors,
 
     this->slew_step = slew_step > 0 ? slew_step : 200;
     this->brakeMode = brakeMode;
+    this->left_motors->set_brake_mode(this->brakeMode);
+    this->right_motors->set_brake_mode(this->brakeMode);
     this->prev_voltages = {0, 0};
 }
 
diff --git a/src/VOSS/controller/BoomerangController.cpp b/src/VOSS/controller/BoomerangController.cpp
index e01627e..32f1c60 100644
--- a/src/VOSS/controller/BoomerangController.cpp
+++ b/src/VOSS/controller/BoomerangController.cpp
@@ -60,6 +60,7 @@ BoomerangController::get_command(bool reverse, bool thru,
         } else {
             // turn to face the finale pose angle if executing a pose movement
             double poseError = target.theta.value() - current_angle;
+
             while (fabs(poseError) > M_PI)
                 poseError -= 2 * M_PI * poseError / fabs(poseError);
             ang_speed = angular_pid(poseError);
diff --git a/src/VOSS/controller/PIDController.cpp b/src/VOSS/controller/PIDController.cpp
index 7206bf2..a850106 100644
--- a/src/VOSS/controller/PIDController.cpp
+++ b/src/VOSS/controller/PIDController.cpp
@@ -55,6 +55,7 @@ PIDController::get_command(bool reverse, bool thru,
         } else {
             // turn to face the finale pose angle if executing a pose movement
             double poseError = target.theta.value() - current_angle;
+
             while (fabs(poseError) > M_PI)
                 poseError -= 2 * M_PI * poseError / fabs(poseError);
             ang_speed = angular_pid(poseError);
diff --git a/src/VOSS/localizer/AbstractLocalizer.cpp b/src/VOSS/localizer/AbstractLocalizer.cpp
index 3037efb..5cea92b 100644
--- a/src/VOSS/localizer/AbstractLocalizer.cpp
+++ b/src/VOSS/localizer/AbstractLocalizer.cpp
@@ -46,7 +46,6 @@ Pose AbstractLocalizer::get_pose() {
     std::unique_lock<pros::Mutex> lock(this->mtx);
     Pose ret = {this->pose.x.load(), this->pose.y.load(),
                 this->pose.theta.load()};
-
     return ret;
 }