From 9d4b585e762d47779fe4f581d2ce370264129326 Mon Sep 17 00:00:00 2001 From: Jacob Williams Date: Tue, 4 Aug 2026 16:08:40 -0400 Subject: [PATCH 1/6] Refactor IMU heading hold in differential_drive Replace reset_heading/turning flags with a single _holding_heading flag and simplify arcade mixing logic. Use early returns, keep wheel-mixing/scaling behavior, and centralize IMU-assisted straight driving: capture heading on entry, hold it while straight, and clear the hold when turning. Improves readability and fixes heading recapture behavior when transitioning between turning and straight driving. --- XRPLib/differential_drive.py | 60 ++++++++++++++++-------------------- 1 file changed, 27 insertions(+), 33 deletions(-) diff --git a/XRPLib/differential_drive.py b/XRPLib/differential_drive.py index cd803e1..b1e2608 100644 --- a/XRPLib/differential_drive.py +++ b/XRPLib/differential_drive.py @@ -80,8 +80,8 @@ def __init__(self, left_motor: EncodedMotor, right_motor: EncodedMotor, imu: IMU self.heading_pid = None self.current_heading = None - self.reset_heading = True - self.turning = False + # True while arcade() is holding a captured heading during straight driving. + self._holding_heading = False if self.imu: self.heading_pid = PID( kp = 0.075, kd=0.001, ) @@ -159,37 +159,31 @@ def arcade(self, straight:float, turn:float): """ if straight == 0 and turn == 0: self.set_effort(0, 0) - else: - scale = max(abs(straight), abs(turn))/(abs(straight) + abs(turn)) - left_speed = (straight - turn)*scale - right_speed = (straight + turn)*scale - - if not self.heading_pid: - # if not using IMU assist to maintain heading, just pass down the left and right motor - # speeds to control movement - self.set_effort(left_speed, right_speed) - else: - # else if IMU assist is enabled, then use the IMU with PID to - # maintain a constant heading while driving. - if turn == 0: - # straight drive requested, then maintain the current heading - if self.turning: - # if previously turning, then clear the turn indicator and reset the course heading - self.reset_heading = True - self.turning = False - - if self.reset_heading: - self.reset_heading = False - self.current_heading = self.imu.get_yaw() - - # use the PID to set the heading correction based on the current heading - heading_correction = self.heading_pid.update(self.current_heading - self.imu.get_yaw()) - - self.set_effort(left_speed - heading_correction, right_speed + heading_correction) - else: - # set the turning indicator and apply the left and right speeds - self.turning = True - self.set_effort(left_speed, right_speed) + return + + # Mix straight and turn, then scale so the faster wheel's magnitude equals the larger + # input. This keeps the straight-to-turn ratio while preventing the sum from clipping. + scale = max(abs(straight), abs(turn)) / (abs(straight) + abs(turn)) + left = (straight - turn) * scale + right = (straight + turn) * scale + + if not self.heading_pid: + # No IMU assist: drive the mixed efforts directly. + self.set_effort(left, right) + return + + if turn != 0: + # Turning: drive directly, and force the next straight frame to recapture heading. + self._holding_heading = False + self.set_effort(left, right) + return + + # Straight with IMU assist: capture the heading once on entry, then hold it with PID. + if not self._holding_heading: + self.current_heading = self.imu.get_yaw() + self._holding_heading = True + correction = self.heading_pid.update(self.current_heading - self.imu.get_yaw()) + self.set_effort(left - correction, right + correction) def reset_encoder_position(self) -> None: """ From 43ba7d41be7e9f6a7a73c3382756f821b883dc89 Mon Sep 17 00:00:00 2001 From: Jacob Williams Date: Tue, 4 Aug 2026 16:11:55 -0400 Subject: [PATCH 2/6] Clear holding heading when drive stopped --- XRPLib/differential_drive.py | 1 + 1 file changed, 1 insertion(+) diff --git a/XRPLib/differential_drive.py b/XRPLib/differential_drive.py index b1e2608..ef32f3a 100644 --- a/XRPLib/differential_drive.py +++ b/XRPLib/differential_drive.py @@ -158,6 +158,7 @@ def arcade(self, straight:float, turn:float): :type turn: float """ if straight == 0 and turn == 0: + self._holding_heading = False self.set_effort(0, 0) return From 6115cd983a5423b72478701ac0add680e9fb44d3 Mon Sep 17 00:00:00 2001 From: Jacob Williams Date: Tue, 4 Aug 2026 16:41:28 -0400 Subject: [PATCH 3/6] Add joystick deadband and PID reset for heading --- XRPLib/differential_drive.py | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/XRPLib/differential_drive.py b/XRPLib/differential_drive.py index ef32f3a..fc28f2d 100644 --- a/XRPLib/differential_drive.py +++ b/XRPLib/differential_drive.py @@ -84,7 +84,7 @@ def __init__(self, left_motor: EncodedMotor, right_motor: EncodedMotor, imu: IMU self._holding_heading = False if self.imu: - self.heading_pid = PID( kp = 0.075, kd=0.001, ) + self.heading_pid = PID(kp=0.075, kd=0.001) def update_voltage_compensation(self) -> float: """ @@ -156,7 +156,15 @@ def arcade(self, straight:float, turn:float): :type straight: float :param turn: The modifier effort (Bounded from -1 to 1) used to skew robot left (positive) or right (negative). :type turn: float + """ + _deadband = 0.1 + # Ignore small resting inputs from joystick drift so the robot sits still near center. + if abs(straight) < _deadband: + straight = 0 + if abs(turn) < _deadband: + turn = 0 + if straight == 0 and turn == 0: self._holding_heading = False self.set_effort(0, 0) @@ -182,6 +190,7 @@ def arcade(self, straight:float, turn:float): # Straight with IMU assist: capture the heading once on entry, then hold it with PID. if not self._holding_heading: self.current_heading = self.imu.get_yaw() + self.heading_pid.clear_history() self._holding_heading = True correction = self.heading_pid.update(self.current_heading - self.imu.get_yaw()) self.set_effort(left - correction, right + correction) From 10538ad237f26dca5133ca3090b889121e3494d1 Mon Sep 17 00:00:00 2001 From: Jacob Williams Date: Wed, 5 Aug 2026 17:55:12 -0400 Subject: [PATCH 4/6] Add NanoXRP-specific heading PID gains --- XRPLib/differential_drive.py | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/XRPLib/differential_drive.py b/XRPLib/differential_drive.py index fc28f2d..e9d2516 100644 --- a/XRPLib/differential_drive.py +++ b/XRPLib/differential_drive.py @@ -84,7 +84,10 @@ def __init__(self, left_motor: EncodedMotor, right_motor: EncodedMotor, imu: IMU self._holding_heading = False if self.imu: - self.heading_pid = PID(kp=0.075, kd=0.001) + if "NanoXRP" in implementation._machine: + self.heading_pid = PID(kp=0.014, kd=0.001) + else: + self.heading_pid = PID(kp=0.064, kd=0.0045) def update_voltage_compensation(self) -> float: """ From aade43669321ce25b87bdb3b99fca6d8e655c98c Mon Sep 17 00:00:00 2001 From: Jacob Williams Date: Wed, 5 Aug 2026 18:06:26 -0400 Subject: [PATCH 5/6] Invert turn input when reversing When driving backward the turn input is inverted so the robot steers toward the joystick the same way as when driving forward. Adds a check in DifferentialDrive to negate `turn` if `straight` is negative before mixing, preserving the existing mixing and scaling logic and preventing reversed steering behavior. --- XRPLib/differential_drive.py | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/XRPLib/differential_drive.py b/XRPLib/differential_drive.py index e9d2516..dcf1e7c 100644 --- a/XRPLib/differential_drive.py +++ b/XRPLib/differential_drive.py @@ -173,6 +173,10 @@ def arcade(self, straight:float, turn:float): self.set_effort(0, 0) return + # Flip the turn when reversing so the robot steers toward the stick either way. + if straight < 0: + turn = -turn + # Mix straight and turn, then scale so the faster wheel's magnitude equals the larger # input. This keeps the straight-to-turn ratio while preventing the sum from clipping. scale = max(abs(straight), abs(turn)) / (abs(straight) + abs(turn)) From 024eac5bcbb476d74f715cedb8b7571227273e30 Mon Sep 17 00:00:00 2001 From: Jacob Williams Date: Fri, 7 Aug 2026 15:54:17 -0400 Subject: [PATCH 6/6] Remove joystick deadband from arcade --- XRPLib/differential_drive.py | 7 ------- 1 file changed, 7 deletions(-) diff --git a/XRPLib/differential_drive.py b/XRPLib/differential_drive.py index dcf1e7c..b2e308d 100644 --- a/XRPLib/differential_drive.py +++ b/XRPLib/differential_drive.py @@ -161,13 +161,6 @@ def arcade(self, straight:float, turn:float): :type turn: float """ - _deadband = 0.1 - # Ignore small resting inputs from joystick drift so the robot sits still near center. - if abs(straight) < _deadband: - straight = 0 - if abs(turn) < _deadband: - turn = 0 - if straight == 0 and turn == 0: self._holding_heading = False self.set_effort(0, 0)