From 61623fe06d337ea0233fde06c7732cb891c438ed Mon Sep 17 00:00:00 2001 From: Jacob Williams Date: Thu, 6 Aug 2026 20:16:25 -0400 Subject: [PATCH 1/4] Refactor encoder rpm/count conversions Centralize and clarify encoder timing and unit conversions. In encoded_motor.py introduce _UPDATE_PERIOD_MS/_UPDATE_HZ, conversion helpers (_counts_per_update_to_rpm, _rpm_to_counts_per_update), and store counts-per-update instead of ambiguous `speed`. Use the update period constant for the virtual timer and for rpm<->counts conversions. Update set_speed/get_speed/inner update logic to use the new helpers and names. In differential_drive.py rename cmpsToRPM to snake_case cmps_to_rpm for naming consistency. Improves readability and correctness of rpm/count calculations. --- XRPLib/differential_drive.py | 6 +++--- XRPLib/encoded_motor.py | 28 +++++++++++++++++++--------- 2 files changed, 22 insertions(+), 12 deletions(-) diff --git a/XRPLib/differential_drive.py b/XRPLib/differential_drive.py index cd803e1..b2ca6c7 100644 --- a/XRPLib/differential_drive.py +++ b/XRPLib/differential_drive.py @@ -126,9 +126,9 @@ def set_speed(self, left_speed: float, right_speed: float) -> None: :type rightSpeed: float """ # Convert from cm/s to RPM - cmpsToRPM = 60 / (math.pi * self.wheel_diam) - self.left_motor.set_speed(left_speed*cmpsToRPM) - self.right_motor.set_speed(right_speed*cmpsToRPM) + cmps_to_rpm = 60 / (math.pi * self.wheel_diam) + self.left_motor.set_speed(left_speed*cmps_to_rpm) + self.right_motor.set_speed(right_speed*cmps_to_rpm) def set_zero_effort_behavior(self, brake_at_zero_effort): diff --git a/XRPLib/encoded_motor.py b/XRPLib/encoded_motor.py index da3ece7..39fbecb 100644 --- a/XRPLib/encoded_motor.py +++ b/XRPLib/encoded_motor.py @@ -10,6 +10,10 @@ class EncodedMotor: ZERO_EFFORT_BREAK = True ZERO_EFFORT_COAST = False + # Speed control runs on a fixed-period timer, and the rpm <-> counts conversions depend on that period. + _UPDATE_PERIOD_MS = 20 + _UPDATE_HZ = 1000 // _UPDATE_PERIOD_MS + _DEFAULT_LEFT_MOTOR_INSTANCE = None _DEFAULT_RIGHT_MOTOR_INSTANCE = None _DEFAULT_MOTOR_THREE_INSTANCE = None @@ -87,12 +91,12 @@ def __init__(self, motor, encoder: Encoder): self.speedController = self.DEFAULT_SPEED_CONTROLLER self.prev_position = 0 - self.speed = 0 + self._counts_per_update = 0 # encoder counts moved in the last update period self.prev_speed = 0 # Use a virtual timer so we can leave the hardware timers up for the user self.updateTimer = Timer(-1) - # If the update timer is not running, start it at 50 Hz (20ms updates) - self.updateTimer.init(period=20, callback=lambda t:self._update()) + # If the update timer is not running, start it at the update rate + self.updateTimer.init(period=self._UPDATE_PERIOD_MS, callback=lambda t:self._update()) def set_effort(self, effort: float): @@ -155,13 +159,20 @@ def reset_encoder_position(self): """ self._encoder.reset_encoder_position() + def _counts_per_update_to_rpm(self, counts: float) -> float: + # counts moved in one update period -> revolutions per minute + return counts * 60 * self._UPDATE_HZ / self._encoder.resolution + + def _rpm_to_counts_per_update(self, rpm: float) -> float: + # revolutions per minute -> counts moved in one update period + return rpm * self._encoder.resolution / (60 * self._UPDATE_HZ) + def get_speed(self) -> float: """ :return: The speed of the motor, in rpm :rtype: float """ - # Convert from counts per 20ms to rpm (60 sec/min, 50 Hz) - return self.speed*(60*50)/self._encoder.resolution + return self._counts_per_update_to_rpm(self._counts_per_update) def set_speed(self, speed_rpm: float = None): """ @@ -182,8 +193,7 @@ def set_speed(self, speed_rpm: float = None): self.prev_speed = speed_rpm - # Convert from rev per min to counts per 20ms (60 sec/min, 50 Hz) - self.target_speed = speed_rpm*self._encoder.resolution/(60*50) + self.target_speed = self._rpm_to_counts_per_update(speed_rpm) def set_speed_controller(self, new_controller: Controller): """ @@ -200,9 +210,9 @@ def _update(self): Non-api method; used for updating motor efforts for speed control """ current_position = self.get_position_counts() - self.speed = current_position - self.prev_position + self._counts_per_update = current_position - self.prev_position if self.target_speed is not None: - error = self.target_speed - self.speed + error = self.target_speed - self._counts_per_update effort = self.speedController.update(error) self._motor.set_effort(effort) self.prev_position = current_position From 35a167f0a2a0bac76b857f410caa105e3125e38d Mon Sep 17 00:00:00 2001 From: Jacob Williams Date: Thu, 6 Aug 2026 21:42:17 -0400 Subject: [PATCH 2/4] Share battery compensation; add motor feedforward Move battery voltage measurement/compensation onto Board (nominal voltage per platform, update_voltage_compensation(), voltage_scale measured at init). DifferentialDrive now reads board.voltage_scale (removed its own redundant voltage logic). EncodedMotor switched to feedforward+P velocity control (kS/kV per platform), holds Board reference, applies voltage_scale to motor effort, and clamps effort to [-1,1]. Minor PID/integral changes to match the new feedforward approach. --- XRPLib/board.py | 22 +++++++++++++++++++++- XRPLib/differential_drive.py | 31 ++++--------------------------- XRPLib/encoded_motor.py | 32 ++++++++++++++++++-------------- 3 files changed, 43 insertions(+), 42 deletions(-) diff --git a/XRPLib/board.py b/XRPLib/board.py index 1a17ead..69a5be8 100644 --- a/XRPLib/board.py +++ b/XRPLib/board.py @@ -7,6 +7,10 @@ class Board: _DEFAULT_BOARD_INSTANCE = None + # Nominal pack voltage the motor gains are tuned against; voltage_scale corrects for the + # pack actually installed (effort is raw PWM duty, so torque scales with voltage). + _nominal_voltage = 4.2 if "NanoXRP" in sys.implementation._machine else 6.0 + @classmethod def get_default_board(cls): """ @@ -28,7 +32,11 @@ def __init__(self, vin_pin="BOARD_VIN_MEASURE", button_pin="BOARD_USER_BUTTON", """ self.on_switch = ADC(Pin(vin_pin)) - + + # Measure the pack once so motor controllers can compensate effort for battery droop. + self.voltage_scale = 1.0 + self.update_voltage_compensation() + self.button = Pin(button_pin, Pin.IN, Pin.PULL_UP) time.sleep(.01) # give some time for the pull up to get to the proper voltage, otherwise the button could read as pressed @@ -131,6 +139,18 @@ def set_rgb_led(self, r:int, g:int, b:int): else: raise NotImplementedError("Board.set_rgb_led not implemented for the XRP Beta") + def update_voltage_compensation(self) -> float: + """ + Re-measure the battery and update voltage_scale, the factor motor controllers apply to + effort so torque stays consistent as the pack drains. Call again after a battery swap. + + :return: The effort scale now in use + :rtype: float + """ + voltage = sum(self.get_battery_voltage() for _ in range(8)) / 8 + self.voltage_scale = min(max(self._nominal_voltage / max(voltage, 3.5), 0.7), 1.6) + return self.voltage_scale + def get_battery_voltage(self, vin_pin="BOARD_VIN_MEASURE") -> float: """ Returns the current battery voltage in volts. diff --git a/XRPLib/differential_drive.py b/XRPLib/differential_drive.py index b2ca6c7..1b39934 100644 --- a/XRPLib/differential_drive.py +++ b/XRPLib/differential_drive.py @@ -68,15 +68,9 @@ def __init__(self, left_motor: EncodedMotor, right_motor: EncodedMotor, imu: IMU else: self.wheel_track = wheel_track - # Effort is raw PWM duty, so torque scales with pack voltage. Gains are tuned against - # nominal_voltage and voltage_scale corrects the duty for the pack actually installed. - if "NanoXRP" in implementation._machine: - self.nominal_voltage = 4.2 - else: - self.nominal_voltage = 6.0 - - self.voltage_scale = 1.0 - self.update_voltage_compensation() + # Battery compensation lives on Board (measured once, shared). straight()/turn() read + # board.voltage_scale so effort tracks the pack as it drains. + self._board = Board.get_default_board() self.heading_pid = None self.current_heading = None @@ -86,23 +80,6 @@ def __init__(self, left_motor: EncodedMotor, right_motor: EncodedMotor, imu: IMU if self.imu: self.heading_pid = PID( kp = 0.075, kd=0.001, ) - def update_voltage_compensation(self) -> float: - """ - Re-measures the battery and updates the effort scale applied to straight() and turn(). - Called at construction; call it again after a battery swap or a long run. - - :return: The effort scale now in use - :rtype: float - """ - if self.nominal_voltage is None: - return self.voltage_scale - - board = Board.get_default_board() - voltage = sum(board.get_battery_voltage() for _ in range(8)) / 8 - self.voltage_scale = min(max(self.nominal_voltage / max(voltage, 3.5), 0.7), 1.6) - - return self.voltage_scale - def set_effort(self, left_effort: float, right_effort: float) -> None: """ Set the raw effort of both motors individually @@ -308,7 +285,7 @@ def _move(self, distance_target: float, heading_target: float, max_effort: float elif correcting and 0 < effort < min_effort: left, right = left * min_effort / effort, right * min_effort / effort - self.set_effort(left * self.voltage_scale, right * self.voltage_scale) + self.set_effort(left * self._board.voltage_scale, right * self._board.voltage_scale) time.sleep(0.01) diff --git a/XRPLib/encoded_motor.py b/XRPLib/encoded_motor.py index 39fbecb..a70e1cb 100644 --- a/XRPLib/encoded_motor.py +++ b/XRPLib/encoded_motor.py @@ -3,6 +3,7 @@ from machine import Timer, Pin from .controller import Controller from .pid import PID +from .board import Board from sys import implementation class EncodedMotor: @@ -74,22 +75,24 @@ def __init__(self, motor, encoder: Encoder): self.brake_at_zero = False self.target_speed = None + + # Velocity control = feedforward (kS breaks stiction, kV per unit speed) plus a + # proportional trim. No integral for now; battery droop is handled by voltage + # compensation instead. kS/kV are in counts-per-update units and need per-robot tuning. if "NanoXRP" in implementation._machine: - self.DEFAULT_SPEED_CONTROLLER = PID( - kp=0.015, - ki=0.06, - kd=0, - max_integral=1/0.06 - ) + self.kS = 0.05 + self.kV = 0.03 + self.DEFAULT_SPEED_CONTROLLER = PID(kp=0.015, ki=0, kd=0) else: - self.DEFAULT_SPEED_CONTROLLER = PID( - kp=0.035, - ki=0.03, - kd=0, - max_integral=50 - ) + self.kS = 0.1 + self.kV = 0.024 + self.DEFAULT_SPEED_CONTROLLER = PID(kp=0.035, ki=0, kd=0) self.speedController = self.DEFAULT_SPEED_CONTROLLER + + # voltage_scale is measured once when Board is constructed; just hold a reference. + self._board = Board.get_default_board() + self.prev_position = 0 self._counts_per_update = 0 # encoder counts moved in the last update period self.prev_speed = 0 @@ -213,6 +216,7 @@ def _update(self): self._counts_per_update = current_position - self.prev_position if self.target_speed is not None: error = self.target_speed - self._counts_per_update - effort = self.speedController.update(error) - self._motor.set_effort(effort) + feedforward = (self.kS if self.target_speed > 0 else -self.kS) + self.kV * self.target_speed + effort = (feedforward + self.speedController.update(error)) * self._board.voltage_scale + self._motor.set_effort(max(-1.0, min(1.0, effort))) self.prev_position = current_position From 23188cfe15e926ba5cfc6c6263678dfada7b5f4c Mon Sep 17 00:00:00 2001 From: Jacob Williams Date: Fri, 7 Aug 2026 01:41:47 -0400 Subject: [PATCH 3/4] Tune encoded motor PID and feedforward defaults Update EncodedMotor defaults: NanoXRP now uses zeroed feedforward (kS/kV = 0.00) with a P-only PID (kp=0.015). Other platforms use kS=0.12, kV=0.02 and a stronger P-only PID (kp=0.1). Also clear prev_speed (set to 0) when stopping so direction is forgotten when the controller is cleared. --- XRPLib/encoded_motor.py | 17 ++++++++--------- 1 file changed, 8 insertions(+), 9 deletions(-) diff --git a/XRPLib/encoded_motor.py b/XRPLib/encoded_motor.py index a70e1cb..6b1456e 100644 --- a/XRPLib/encoded_motor.py +++ b/XRPLib/encoded_motor.py @@ -76,17 +76,15 @@ def __init__(self, motor, encoder: Encoder): self.target_speed = None - # Velocity control = feedforward (kS breaks stiction, kV per unit speed) plus a - # proportional trim. No integral for now; battery droop is handled by voltage - # compensation instead. kS/kV are in counts-per-update units and need per-robot tuning. + # Velocity control = feedforward (kS breaks stiction, kV per unit speed) plus a proportional trim. if "NanoXRP" in implementation._machine: - self.kS = 0.05 - self.kV = 0.03 - self.DEFAULT_SPEED_CONTROLLER = PID(kp=0.015, ki=0, kd=0) + self.kS = 0.00 + self.kV = 0.00 + self.DEFAULT_SPEED_CONTROLLER = PID(kp=0.015) else: - self.kS = 0.1 - self.kV = 0.024 - self.DEFAULT_SPEED_CONTROLLER = PID(kp=0.035, ki=0, kd=0) + self.kS = 0.12 + self.kV = 0.02 + self.DEFAULT_SPEED_CONTROLLER = PID(kp=0.1) self.speedController = self.DEFAULT_SPEED_CONTROLLER @@ -187,6 +185,7 @@ def set_speed(self, speed_rpm: float = None): """ if speed_rpm is None or speed_rpm == 0: self.target_speed = None + self.prev_speed = 0 # forget direction; the controller is cleared right below self.set_effort(0) self.speedController.clear_history() return From cdf1fecf79e9475f24affd2da071703d824c17b5 Mon Sep 17 00:00:00 2001 From: Jacob Williams Date: Mon, 10 Aug 2026 18:40:58 -0400 Subject: [PATCH 4/4] Tune NanoXRP motor feedforward and PID gains --- XRPLib/encoded_motor.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/XRPLib/encoded_motor.py b/XRPLib/encoded_motor.py index 6b1456e..b5fb511 100644 --- a/XRPLib/encoded_motor.py +++ b/XRPLib/encoded_motor.py @@ -78,9 +78,9 @@ def __init__(self, motor, encoder: Encoder): # Velocity control = feedforward (kS breaks stiction, kV per unit speed) plus a proportional trim. if "NanoXRP" in implementation._machine: - self.kS = 0.00 - self.kV = 0.00 - self.DEFAULT_SPEED_CONTROLLER = PID(kp=0.015) + self.kS = 0.1 + self.kV = 0.00122 + self.DEFAULT_SPEED_CONTROLLER = PID(kp=0.005) else: self.kS = 0.12 self.kV = 0.02