Use length instead of lengthSquared checking for pure pid

nycrat · nycrat · commit 1ebd776cc0f1 · 2026-05-26T12:54:19.000-07:00
diff --git a/src/software/embedded/motion_control/position_controller.cpp b/src/software/embedded/motion_control/position_controller.cpp
@@ -7,7 +7,7 @@ Vector PositionController::step(const Point& position,
     const Vector distance_from_destination =
         target_trajectory.getDestination() - position;
 
-    if (distance_from_destination.lengthSquared() < LINEAR_PURE_PID_THRESHOLD_METERS)
+    if (distance_from_destination.length() < LINEAR_PURE_PID_THRESHOLD_METERS)
     {
         // if target destination is close enough, use pure PID for velocity
         return Vector{x_pid_close_.step(distance_from_destination.x(), delta_time),

Original file line number	Diff line number	Diff line change
`@@ -7,7 +7,7 @@ Vector PositionController::step(const Point& position,`
`7`	`7`	`const Vector distance_from_destination =`
`8`	`8`	`target_trajectory.getDestination() - position;`
`9`	`9`
`10`		`- if (distance_from_destination.lengthSquared() < LINEAR_PURE_PID_THRESHOLD_METERS)`
	`10`	`+ if (distance_from_destination.length() < LINEAR_PURE_PID_THRESHOLD_METERS)`
`11`	`11`	`{`
`12`	`12`	`// if target destination is close enough, use pure PID for velocity`
`13`	`13`	`return Vector{x_pid_close_.step(distance_from_destination.x(), delta_time),`