fix boomerang (#112)

* fix boomerang * Committing clang-format changes --------- Co-authored-by: github-actions <41898282+github-actions[bot]@users.noreply.github.com>
purduesigbots · Nov 23, 2024 · 7ac5410 · 7ac5410
1 parent 66e1e2b
commit 7ac5410
Show file tree

Hide file tree

Showing 2 changed files with 9 additions and 4 deletions.
diff --git a/[email protected] b/[email protected]
diff --git a/src/VOSS/controller/BoomerangController.cpp b/src/VOSS/controller/BoomerangController.cpp
@@ -28,16 +28,21 @@ BoomerangController::get_command(bool reverse, bool thru,
     double dy = target.y - current_pos.y;
 
     double distance_error = sqrt(dx * dx + dy * dy);
-    double at = voss::to_radians(target.theta.value());
+    double at = target.theta.value();
 
     this->carrotPoint = {this->target.x - distance_error * cos(at) * lead_pct,
                          this->target.y - distance_error * sin(at) * lead_pct,
                          target.theta};
-    double current_angle =
-        this->l->get_orientation_rad() + (reverse ? M_PI : 0);
+    dx = carrotPoint.x - current_pos.x;
+    dy = carrotPoint.y - current_pos.y;
+    double current_angle = this->l->get_orientation_rad();
 
     double angle_error;
-    angle_error = atan2(dy, dx) - current_angle;
+    if (!reverse) {
+        angle_error = atan2(dy, dx) - current_angle;
+    } else {
+        angle_error = atan2(-dy, -dx) - current_angle;
+    }
 
     angle_error = voss::norm_delta(angle_error);