Implement turn direction for swing controller

purduesigbots · Feb 14, 2024 · f77b2e7 · f77b2e7
1 parent 770f506
commit f77b2e7
Showing 1 changed file with 16 additions and 1 deletion.
diff --git 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}