From f77b2e7ff30a51fd4aa9226a9d7a915c70a19724 Mon Sep 17 00:00:00 2001
From: Rocky14683 <rocky10208@gmail.com>
Date: Tue, 13 Feb 2024 21:55:05 -0500
Subject: [PATCH] Implement turn direction for swing controller

---
 src/VOSS/controller/SwingController.cpp | 17 ++++++++++++++++-
 1 file changed, 16 insertions(+), 1 deletion(-)

diff --git a/src/VOSS/controller/SwingController.cpp b/src/VOSS/controller/SwingController.cpp
index 148a610..ec9f507 100644
--- a/src/VOSS/controller/SwingController.cpp
+++ b/src/VOSS/controller/SwingController.cpp
@@ -62,13 +62,28 @@ SwingController::get_angular_command(bool reverse, bool thru,
         return chassis::ChassisCommand{chassis::Stop{}};
     }
 
+    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;
+        }
+    }
+
     double ang_speed = angular_pid(angular_error);
     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) {
         if (reverse) {
             command = std::signbit(ang_speed) ? chassis::Swing{-ang_speed, 0}