From 9d8cd9adc31f09fe41604ca0bbdfb9db66918e4b Mon Sep 17 00:00:00 2001 From: Jacob Williams Date: Fri, 14 Mar 2025 14:39:14 -0400 Subject: [PATCH 01/11] Added support for flipping encoder direction --- XRPLib/encoder.py | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/XRPLib/encoder.py b/XRPLib/encoder.py index 414b973..efa7d89 100644 --- a/XRPLib/encoder.py +++ b/XRPLib/encoder.py @@ -9,7 +9,7 @@ class Encoder: _counts_per_motor_shaft_revolution = 12 resolution = _counts_per_motor_shaft_revolution * _gear_ratio # 585 - def __init__(self, index, encAPin: int|str, encBPin: int|str): + def __init__(self, index, encAPin: int|str, encBPin: int|str, flip_dir:bool=False): """ Uses the on board PIO State Machine to keep track of encoder positions. Only 4 encoders can be instantiated this way. @@ -28,6 +28,7 @@ def __init__(self, index, encAPin: int|str, encBPin: int|str): self.sm = rp2.StateMachine(index, self._encoder, in_base=basePin) self.reset_encoder_position() self.sm.active(1) + self.flip_dir = flip_dir def reset_encoder_position(self): """ @@ -52,6 +53,10 @@ def get_position_counts(self): counts = self.sm.get() if(counts > 2**31): counts -= 2**32 + + if self.flip_dir: + counts *= -1 + return counts def get_position(self): From ba8d94567c0b66b03c6b6ef47e5cf1206d04beeb Mon Sep 17 00:00:00 2001 From: Jacob Williams Date: Wed, 19 Nov 2025 17:31:39 -0500 Subject: [PATCH 02/11] Add conditional support for fourth motor based on hardware Updated defaults.py, encoded_motor.py, and resetbot.py to only initialize and use the fourth motor if Pin.board.MOTOR_4_IN_1 is available. This improves compatibility with hardware configurations that do not include a fourth motor. --- XRPLib/defaults.py | 3 ++- XRPLib/encoded_motor.py | 4 ++-- XRPLib/resetbot.py | 8 +++++++- 3 files changed, 11 insertions(+), 4 deletions(-) diff --git a/XRPLib/defaults.py b/XRPLib/defaults.py index 38216c8..ab88470 100644 --- a/XRPLib/defaults.py +++ b/XRPLib/defaults.py @@ -18,7 +18,6 @@ left_motor = EncodedMotor.get_default_encoded_motor(index=1) right_motor = EncodedMotor.get_default_encoded_motor(index=2) motor_three = EncodedMotor.get_default_encoded_motor(index=3) -motor_four = EncodedMotor.get_default_encoded_motor(index=4) imu = IMU.get_default_imu() drivetrain = DifferentialDrive.get_default_differential_drive() rangefinder = Rangefinder.get_default_rangefinder() @@ -28,6 +27,8 @@ webserver = Webserver.get_default_webserver() board = Board.get_default_board() +if hasattr(Pin.board, "MOTOR_4_IN_1"): + motor_four = EncodedMotor.get_default_encoded_motor(index=4) if hasattr(Pin.board, "SERVO_3"): servo_three = Servo.get_default_servo(index=3) if hasattr(Pin.board, "SERVO_4"): diff --git a/XRPLib/encoded_motor.py b/XRPLib/encoded_motor.py index de2fa6f..78aea6f 100644 --- a/XRPLib/encoded_motor.py +++ b/XRPLib/encoded_motor.py @@ -1,6 +1,6 @@ from .motor import SinglePWMMotor, DualPWMMotor from .encoder import Encoder -from machine import Timer +from machine import Timer, Pin from .controller import Controller from .pid import PID import sys @@ -51,7 +51,7 @@ def get_default_encoded_motor(cls, index:int = 1): Encoder(2, "MOTOR_3_ENCODER_A", "MOTOR_3_ENCODER_B") ) motor = cls._DEFAULT_MOTOR_THREE_INSTANCE - elif index == 4: + elif index == 4 and hasattr(Pin.board, "MOTOR_4_IN_1"): if cls._DEFAULT_MOTOR_FOUR_INSTANCE is None: cls._DEFAULT_MOTOR_FOUR_INSTANCE = cls( MotorImplementation("MOTOR_4_IN_1", "MOTOR_4_IN_2"), diff --git a/XRPLib/resetbot.py b/XRPLib/resetbot.py index 7c9d321..a068d36 100644 --- a/XRPLib/resetbot.py +++ b/XRPLib/resetbot.py @@ -1,3 +1,4 @@ +from machine import Pin import sys """ A simple file for shutting off all of the motors after a program gets interrupted from the REPL. @@ -6,8 +7,13 @@ def reset_motors(): from XRPLib.encoded_motor import EncodedMotor + + number_of_motors = 3 + if hasattr(Pin.board, "MOTOR_4_IN_1"): + number_of_motors = 4 + # using the EncodedMotor since the default drivetrain uses the IMU and takes 3 seconds to init - for i in range(4): + for i in range(number_of_motors): motor = EncodedMotor.get_default_encoded_motor(i+1) motor.set_speed(0) motor.reset_encoder_position() From 04958efbc0937f662f7f70a21aef222f78827d50 Mon Sep 17 00:00:00 2001 From: Jacob Williams Date: Wed, 19 Nov 2025 18:29:19 -0500 Subject: [PATCH 03/11] Update encoded_motor.py --- 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 78aea6f..2568fc5 100644 --- a/XRPLib/encoded_motor.py +++ b/XRPLib/encoded_motor.py @@ -25,10 +25,10 @@ def get_default_encoded_motor(cls, index:int = 1): :type index: int """ - if "RP2350" in sys.implementation._machine: - MotorImplementation = DualPWMMotor - else: + if "Beta" in sys.implementation._machine: MotorImplementation = SinglePWMMotor + else: + MotorImplementation = DualPWMMotor if index == 1: if cls._DEFAULT_LEFT_MOTOR_INSTANCE is None: From 6bb8050d9c1e786e4ddabf2d1b15570df04ee2ac Mon Sep 17 00:00:00 2001 From: Jacob Williams Date: Wed, 26 Nov 2025 17:38:10 -0500 Subject: [PATCH 04/11] Update reflectance.py --- XRPLib/reflectance.py | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/XRPLib/reflectance.py b/XRPLib/reflectance.py index c986952..f5a866d 100644 --- a/XRPLib/reflectance.py +++ b/XRPLib/reflectance.py @@ -13,7 +13,7 @@ def get_default_reflectance(cls): cls._DEFAULT_REFLECTANCE_INSTANCE = cls() return cls._DEFAULT_REFLECTANCE_INSTANCE - def __init__(self, leftPin: int|str = "LINE_L", rightPin: int|str = "LINE_R"): + def __init__(self, leftPin: int|str = "LINE_L", middlePin: int|str = "LINE_M", rightPin: int|str = "LINE_R"): """ Implements for a reflectance sensor using the built in 12-bit ADC. Reads from analog in and converts to a float from 0 (white) to 1 (black) @@ -24,6 +24,7 @@ def __init__(self, leftPin: int|str = "LINE_L", rightPin: int|str = "LINE_R"): :type rightPin: int """ self._leftReflectance = ADC(Pin(leftPin)) + self._middleReflectance = ADC(Pin(middlePin)) self._rightReflectance = ADC(Pin(rightPin)) self.MAX_ADC_VALUE: int = 65536 @@ -39,6 +40,14 @@ def get_left(self) -> float: : rtype: float """ return self._get_value(self._leftReflectance) + + def get_middle(self) -> float: + """ + Gets the the reflectance of the left reflectance sensor + : return: The reflectance ranging from 0 (white) to 1 (black) + : rtype: float + """ + return self._get_value(self._middleReflectance) def get_right(self) -> float: """ From 07e2d2659f211ff4e1b0dd6b5662b3b747d3f795 Mon Sep 17 00:00:00 2001 From: Jacob Williams Date: Wed, 11 Mar 2026 21:06:00 -0400 Subject: [PATCH 05/11] Use sys.implementation, add encoder flip, fix encoder pin check Import sys.implementation and make defaults platform-aware: instantiate motor_four conditionally, remove duplicate instantiation, and choose a NanoXRP-specific drivetrain with explicit wheel dimensions. Replace sys.implementation usage in EncodedMotor with a direct import. Extend Encoder to accept a flip_dir flag, normalize pin ordering at init, and invert returned counts when flip_dir is true to allow reversing encoder direction. --- XRPLib/defaults.py | 13 ++++++++++--- XRPLib/encoded_motor.py | 4 ++-- XRPLib/encoder.py | 22 ++++++++++++++++------ 3 files changed, 28 insertions(+), 11 deletions(-) diff --git a/XRPLib/defaults.py b/XRPLib/defaults.py index ab88470..4db40a9 100644 --- a/XRPLib/defaults.py +++ b/XRPLib/defaults.py @@ -9,6 +9,7 @@ from .servo import Servo from .webserver import Webserver from machine import Pin +from sys import implementation """ A simple file that constructs all of the default objects for the XRP robot @@ -18,8 +19,16 @@ left_motor = EncodedMotor.get_default_encoded_motor(index=1) right_motor = EncodedMotor.get_default_encoded_motor(index=2) motor_three = EncodedMotor.get_default_encoded_motor(index=3) +if hasattr(Pin.board, "MOTOR_4_IN_1"): + motor_four = EncodedMotor.get_default_encoded_motor(index=4) + imu = IMU.get_default_imu() -drivetrain = DifferentialDrive.get_default_differential_drive() + +if "NanoXRP" in implementation._machine: + drivetrain = DifferentialDrive.get_default_differential_drive(wheel_diam:float = 3.2, wheel_track:float = 7.8) +else: + drivetrain = DifferentialDrive.get_default_differential_drive() + rangefinder = Rangefinder.get_default_rangefinder() reflectance = Reflectance.get_default_reflectance() servo_one = Servo.get_default_servo(index=1) @@ -27,8 +36,6 @@ webserver = Webserver.get_default_webserver() board = Board.get_default_board() -if hasattr(Pin.board, "MOTOR_4_IN_1"): - motor_four = EncodedMotor.get_default_encoded_motor(index=4) if hasattr(Pin.board, "SERVO_3"): servo_three = Servo.get_default_servo(index=3) if hasattr(Pin.board, "SERVO_4"): diff --git a/XRPLib/encoded_motor.py b/XRPLib/encoded_motor.py index 2568fc5..b9c9700 100644 --- a/XRPLib/encoded_motor.py +++ b/XRPLib/encoded_motor.py @@ -3,7 +3,7 @@ from machine import Timer, Pin from .controller import Controller from .pid import PID -import sys +from sys import implementation class EncodedMotor: @@ -25,7 +25,7 @@ def get_default_encoded_motor(cls, index:int = 1): :type index: int """ - if "Beta" in sys.implementation._machine: + if "Beta" in implementation._machine: MotorImplementation = SinglePWMMotor else: MotorImplementation = DualPWMMotor diff --git a/XRPLib/encoder.py b/XRPLib/encoder.py index 414b973..159e93f 100644 --- a/XRPLib/encoder.py +++ b/XRPLib/encoder.py @@ -9,7 +9,7 @@ class Encoder: _counts_per_motor_shaft_revolution = 12 resolution = _counts_per_motor_shaft_revolution * _gear_ratio # 585 - def __init__(self, index, encAPin: int|str, encBPin: int|str): + def __init__(self, index, encAPin: int|str, encBPin: int|str, flip_dir:bool=False): """ Uses the on board PIO State Machine to keep track of encoder positions. Only 4 encoders can be instantiated this way. @@ -21,14 +21,20 @@ def __init__(self, index, encAPin: int|str, encBPin: int|str): :param encBPin: The pin the right reflectance sensor is connected to :type encBPin: int """ - # if(abs(encAPin - encBPin) != 1): - # raise Exception("Encoder pins must be successive!") - basePin = machine.Pin(min(encAPin, encBPin), machine.Pin.IN) - nextPin = machine.Pin(max(encAPin, encBPin), machine.Pin.IN) + + basePin = machine.Pin(encAPin, machine.Pin.IN) + nextPin = machine.Pin(encBPin, machine.Pin.IN) + + #nextPin must be higher than basePin. Pins must be successive. + if (str(basePin) > str(nextPin)): + basePin = machine.Pin(encBPin, machine.Pin.IN) + nextPin = machine.Pin(encAPin, machine.Pin.IN) + self.sm = rp2.StateMachine(index, self._encoder, in_base=basePin) self.reset_encoder_position() self.sm.active(1) - + self.flip_dir = flip_dir + def reset_encoder_position(self): """ Resets the encoder position to 0 @@ -52,6 +58,10 @@ def get_position_counts(self): counts = self.sm.get() if(counts > 2**31): counts -= 2**32 + + if self.flip_dir: + counts *= -1 + return counts def get_position(self): From 94daeae18969c779a2ef404983d23326a186e855 Mon Sep 17 00:00:00 2001 From: Jacob Williams Date: Thu, 12 Mar 2026 02:27:42 -0400 Subject: [PATCH 06/11] Support NanoXRP drivetrain and encoder Adjust defaults and encoder behavior for NanoXRP hardware. In defaults.py the drivetrain is constructed directly for NanoXRP with left_motor, right_motor, imu and corrected wheel_diam (3.46) and wheel_track (7.8) instead of the invalid annotated call. In encoder.py the module now imports sys.implementation and selects a NanoXRP-specific gear_ratio (68) while keeping counts_per_motor_shaft_revolution at 12; other platforms retain the original gear ratio. These changes adapt drivetrain and encoder resolution to the NanoXRP platform. --- XRPLib/defaults.py | 2 +- XRPLib/encoder.py | 10 ++++++++-- 2 files changed, 9 insertions(+), 3 deletions(-) diff --git a/XRPLib/defaults.py b/XRPLib/defaults.py index 4db40a9..52c474d 100644 --- a/XRPLib/defaults.py +++ b/XRPLib/defaults.py @@ -25,7 +25,7 @@ imu = IMU.get_default_imu() if "NanoXRP" in implementation._machine: - drivetrain = DifferentialDrive.get_default_differential_drive(wheel_diam:float = 3.2, wheel_track:float = 7.8) + drivetrain = DifferentialDrive(left_motor, right_motor, imu, wheel_diam=3.46, wheel_track=7.8) else: drivetrain = DifferentialDrive.get_default_differential_drive() diff --git a/XRPLib/encoder.py b/XRPLib/encoder.py index 159e93f..4924254 100644 --- a/XRPLib/encoder.py +++ b/XRPLib/encoder.py @@ -3,10 +3,16 @@ import machine import rp2 import time +from sys import implementation class Encoder: - _gear_ratio = (30/14) * (28/16) * (36/9) * (26/8) # 48.75 - _counts_per_motor_shaft_revolution = 12 + if "NanoXRP" in implementation._machine: + _gear_ratio = (68/1) + _counts_per_motor_shaft_revolution = 12 + else: + _gear_ratio = (30/14) * (28/16) * (36/9) * (26/8) # 48.75 + _counts_per_motor_shaft_revolution = 12 + resolution = _counts_per_motor_shaft_revolution * _gear_ratio # 585 def __init__(self, index, encAPin: int|str, encBPin: int|str, flip_dir:bool=False): From 214021455eaacf36ab6eac7a8fa8722ce77edb66 Mon Sep 17 00:00:00 2001 From: Jacob Williams Date: Mon, 23 Mar 2026 12:41:42 -0400 Subject: [PATCH 07/11] Guard middle reflectance pin; Updated docstring add a runtime guard for the middle reflectance sensor to avoid crashes on older XRP boards where LINE_M (middle pin) isn't defined. Docstrings updated to reflect the widened pin type. --- XRPLib/reflectance.py | 15 ++++++++++++--- 1 file changed, 12 insertions(+), 3 deletions(-) diff --git a/XRPLib/reflectance.py b/XRPLib/reflectance.py index f5a866d..ec1fde2 100644 --- a/XRPLib/reflectance.py +++ b/XRPLib/reflectance.py @@ -19,14 +19,23 @@ def __init__(self, leftPin: int|str = "LINE_L", middlePin: int|str = "LINE_M", r Reads from analog in and converts to a float from 0 (white) to 1 (black) :param leftPin: The pin the left reflectance sensor is connected to - :type leftPin: int + :type leftPin: int|str + :param middlePin: The pin the middle reflectance sensor is connected to + :type middlePin: int|str :param rightPin: The pin the right reflectance sensor is connected to - :type rightPin: int + :type rightPin: int|str """ self._leftReflectance = ADC(Pin(leftPin)) - self._middleReflectance = ADC(Pin(middlePin)) self._rightReflectance = ADC(Pin(rightPin)) + # Guard for middlePin to prevent crashes on older XRP boards where LINE_M isn't defined + self._middleReflectance = None + if isinstance(middlePin, int): + self._middleReflectance = ADC(Pin(middlePin)) + elif hasattr(Pin.board, middlePin): + self._middleReflectance = ADC(Pin(middlePin)) + + self.MAX_ADC_VALUE: int = 65536 def _get_value(self, sensor: ADC) -> float: From 3c2538fd9f37504bfe30b098bdfabe1d1f21861d Mon Sep 17 00:00:00 2001 From: Jacob Williams Date: Mon, 23 Mar 2026 12:48:56 -0400 Subject: [PATCH 08/11] Order encoder pins by numeric GPIO id Ensure the encoder's base and next pins are determined by numeric GPIO ID instead of comparing Pin string representations. Introduce local pA/pB pins and a helper _get_pin_id that extracts the numeric GPIO from machine.Pin (with a regex fallback to id()). Update docstring types for encAPin/encBPin to allow int/str. This makes the in_base selection for the RP2 StateMachine more reliable across MicroPython ports. --- XRPLib/encoder.py | 36 ++++++++++++++++++++++++++++-------- 1 file changed, 28 insertions(+), 8 deletions(-) diff --git a/XRPLib/encoder.py b/XRPLib/encoder.py index 4924254..60e6e07 100644 --- a/XRPLib/encoder.py +++ b/XRPLib/encoder.py @@ -23,24 +23,44 @@ def __init__(self, index, encAPin: int|str, encBPin: int|str, flip_dir:bool=Fals :param index: The index of the state machine to be used, indexed 0-3. :type index: int :param encAPin: The pin the left reflectance sensor is connected to - :type encAPin: int + :type encAPin: int/str :param encBPin: The pin the right reflectance sensor is connected to - :type encBPin: int + :type encBPin: int/str """ - basePin = machine.Pin(encAPin, machine.Pin.IN) - nextPin = machine.Pin(encBPin, machine.Pin.IN) + pA = machine.Pin(encAPin, machine.Pin.IN) + pB = machine.Pin(encBPin, machine.Pin.IN) - #nextPin must be higher than basePin. Pins must be successive. - if (str(basePin) > str(nextPin)): - basePin = machine.Pin(encBPin, machine.Pin.IN) - nextPin = machine.Pin(encAPin, machine.Pin.IN) + # PIO in_base requires successive pins. We must ensure basePin is the lower GPIO number. + # We extract the GPIO number from the Pin object to perform a reliable numeric comparison. + if self._get_pin_id(pA) > self._get_pin_id(pB): + basePin = pB + nextPin = pA + else: + basePin = pA + nextPin = pB self.sm = rp2.StateMachine(index, self._encoder, in_base=basePin) self.reset_encoder_position() self.sm.active(1) self.flip_dir = flip_dir + def _get_pin_id(self, pin): + """ + Helper to extract the numeric GPIO ID from a machine.Pin object. + Works across different MicroPython port string representations. + """ + # String representation is usually "Pin(GPIO16, mode=IN)" or "Pin(16)" + # We look for the numeric part associated with the GPIO. + s = str(pin) + import re + match = re.search(r'(\d+)', s) + if match: + return int(match.group(1)) + # Fallback to a basic hash or id if regex fails, though + # on RP2/MicroPython, the numeric ID is standard. + return id(pin) + def reset_encoder_position(self): """ Resets the encoder position to 0 From 537b9ee71d2dafe3523cb77f43c4e49d08930935 Mon Sep 17 00:00:00 2001 From: Jacob Williams Date: Fri, 3 Apr 2026 18:04:06 -0400 Subject: [PATCH 09/11] Adjust control/sensor behavior for NanoXRP Detect NanoXRP via sys.implementation and apply platform-specific tweaks: import implementation in motor/imu/differential_drive, skip creating heading PID on NanoXRP, and use different PID gains for main/secondary controllers for distance/rotation on NanoXRP versus other platforms. Also flip IMU yaw sign on NanoXRP and invert motor flip_dir handling to correct orientation differences. These changes adapt control and sensor behavior to NanoXRP's hardware quirks and dynamics. --- XRPLib/differential_drive.py | 116 ++++++++++++++++++++++------------- XRPLib/encoded_motor.py | 21 +++++-- XRPLib/imu.py | 5 ++ XRPLib/motor.py | 8 ++- 4 files changed, 102 insertions(+), 48 deletions(-) diff --git a/XRPLib/differential_drive.py b/XRPLib/differential_drive.py index e3fbd5f..bf946f2 100644 --- a/XRPLib/differential_drive.py +++ b/XRPLib/differential_drive.py @@ -5,6 +5,7 @@ from .timeout import Timeout import time import math +from sys import implementation class DifferentialDrive: @@ -58,7 +59,8 @@ def __init__(self, left_motor: EncodedMotor, right_motor: EncodedMotor, imu: IMU if self.imu: # if the IMU is initialized, then create a PID controller that can be used # to maintain a constant heading when driving - self.heading_pid = PID( kp = 0.075, kd=0.001, ) + if not "NanoXRP" in implementation._machine: + self.heading_pid = PID( kp = 0.075, kd=0.001, ) def set_effort(self, left_effort: float, right_effort: float) -> None: """ @@ -198,24 +200,40 @@ def straight(self, distance: float, max_effort: float = 0.5, timeout: float = No starting_left = self.get_left_encoder_position() starting_right = self.get_right_encoder_position() - - if main_controller is None: - main_controller = PID( - kp = 0.1, - ki = 0.04, - kd = 0.04, - min_output = 0.3, - max_output = max_effort, - max_integral = 10, - tolerance = 0.25, - tolerance_count = 3, - ) - - # Secondary controller to keep encoder values in sync - if secondary_controller is None: - secondary_controller = PID( - kp = 0.075, kd=0.001, - ) + if "NanoXRP" in implementation._machine: + if main_controller is None: + main_controller = PID( + kp = 0.32, + kd = 0.0184, + min_output = 0.1, + max_output = max_effort, + tolerance = 0.25, + tolerance_count = 3, + ) + + # Secondary controller to keep encoder values in sync + if secondary_controller is None: + secondary_controller = PID( + kp = 0.012, kd=0.00129, + ) + else: + if main_controller is None: + main_controller = PID( + kp = 0.1, + ki = 0.04, + kd = 0.04, + min_output = 0.3, + max_output = max_effort, + max_integral = 10, + tolerance = 0.25, + tolerance_count = 3, + ) + + # Secondary controller to keep encoder values in sync + if secondary_controller is None: + secondary_controller = PID( + kp = 0.075, kd=0.001, + ) if self.imu is not None: # record current heading to maintain it @@ -284,28 +302,44 @@ def turn(self, turn_degrees: float, max_effort: float = 0.5, timeout: float = No time_out = Timeout(timeout) starting_left = self.get_left_encoder_position() starting_right = self.get_right_encoder_position() - - if main_controller is None: - main_controller = PID( - # kp = 0.2, - # ki = 0.004, - # kd = 0.0036, - kd = 0.0036 + 0.0034 * (max(max_effort, 0.5) - 0.5) * 2, - kp = 0.2, - ki = 0.004, - #kd = 0.007, - min_output = 0.1, - max_output = max_effort, - max_integral = 30, - tolerance = 1, - tolerance_count = 3 - ) - - # Secondary controller to keep encoder values in sync - if secondary_controller is None: - secondary_controller = PID( - kp = 0.25, - ) + + if "NanoXRP" in implementation._machine: + if main_controller is None: + main_controller = PID( + kp = 0.016, + kd = 0.0008, + min_output = 0.05, + max_output = max_effort, + tolerance = 1, + tolerance_count = 3 + ) + # Secondary controller to keep encoder values in sync + if secondary_controller is None: + secondary_controller = PID( + kp = 0.32, + kd = 0.0184, + ) + else: + if main_controller is None: + main_controller = PID( + # kp = 0.2, + # ki = 0.004, + # kd = 0.0036, + kd = 0.0036 + 0.0034 * (max(max_effort, 0.5) - 0.5) * 2, + kp = 0.2, + ki = 0.004, + #kd = 0.007, + min_output = 0.1, + max_output = max_effort, + max_integral = 30, + tolerance = 1, + tolerance_count = 3 + ) + # Secondary controller to keep encoder values in sync + if secondary_controller is None: + secondary_controller = PID( + kp = 0.25, + ) if use_imu and (self.imu is not None): turn_degrees += self.imu.get_yaw() diff --git a/XRPLib/encoded_motor.py b/XRPLib/encoded_motor.py index b9c9700..64e2733 100644 --- a/XRPLib/encoded_motor.py +++ b/XRPLib/encoded_motor.py @@ -70,12 +70,21 @@ def __init__(self, motor, encoder: Encoder): self.brake_at_zero = False self.target_speed = None - self.DEFAULT_SPEED_CONTROLLER = PID( - kp=0.035, - ki=0.03, - kd=0, - max_integral=50 - ) + if "NanoXRP" in implementation._machine: + self.DEFAULT_SPEED_CONTROLLER = PID( + kp=0.015, + ki=0.06, + kd=0, + max_integral=1/0.06 + ) + else: + self.DEFAULT_SPEED_CONTROLLER = PID( + kp=0.035, + ki=0.03, + kd=0, + max_integral=50 + ) + self.speedController = self.DEFAULT_SPEED_CONTROLLER self.prev_position = 0 self.speed = 0 diff --git a/XRPLib/imu.py b/XRPLib/imu.py index cd7acbf..e865f73 100644 --- a/XRPLib/imu.py +++ b/XRPLib/imu.py @@ -11,6 +11,7 @@ # Import wrapped in a try/except so that autodoc generation can process properly pass from machine import I2C, Pin, Timer, disable_irq, enable_irq +from sys import implementation import time, math class IMU(): @@ -552,6 +553,10 @@ def _update_imu_readings(self): delta_roll = self.irq_v[1][1] / 1000 / self.timer_frequency delta_yaw = self.irq_v[1][2] / 1000 / self.timer_frequency + # Flip and swap for NanoXRP + if "NanoXRP" in implementation._machine: + delta_yaw = -delta_yaw + state = disable_irq() self.running_pitch += delta_pitch self.running_roll += delta_roll diff --git a/XRPLib/motor.py b/XRPLib/motor.py index 2c5d479..651d0cf 100644 --- a/XRPLib/motor.py +++ b/XRPLib/motor.py @@ -1,4 +1,5 @@ from machine import Pin, PWM +from sys import implementation class SinglePWMMotor: @@ -55,7 +56,12 @@ class DualPWMMotor: """ def __init__(self, in1_pwm_forward: int|str, in2_pwm_backward: int|str, flip_dir:bool=False): - self.flip_dir = flip_dir + + if "NanoXRP" in implementation._machine: + self.flip_dir = flip_dir ^ True + else: + self.flip_dir = flip_dir + self._MAX_PWM = 65535 # Motor holds when actually at full power self._in1ForwardPin = PWM(Pin(in1_pwm_forward, Pin.OUT)) From 2a24ffb5badef24d1f67b65217ecca076905fa31 Mon Sep 17 00:00:00 2001 From: Jacob Williams Date: Sat, 4 Apr 2026 02:17:16 -0400 Subject: [PATCH 10/11] Add Buzzer driver and include in defaults Introduce XRPLib/buzzer.py: a Buzzer class for XRP robots that plays notes and songs (blocking and non-blocking). Features note parsing (naturals, sharps, flats), tempo/duration conversion, frequency computation, PWM control on default pin 13, a Timer-based non-blocking playback path, and several built-in songs. Also update XRPLib/defaults.py to import Buzzer and expose a default buzzer instance (buzzer) when running on NanoXRP via Buzzer.get_default_buzzer(). --- XRPLib/buzzer.py | 373 +++++++++++++++++++++++++++++++++++++++++++++ XRPLib/defaults.py | 6 +- 2 files changed, 378 insertions(+), 1 deletion(-) create mode 100644 XRPLib/buzzer.py diff --git a/XRPLib/buzzer.py b/XRPLib/buzzer.py new file mode 100644 index 0000000..979e7eb --- /dev/null +++ b/XRPLib/buzzer.py @@ -0,0 +1,373 @@ +# XRP Buzzer Library +# An easy-to-use library for playing notes and songs on the XRP buzzer +# Designed to be simple and fun for kids! + +import machine +import time +import urandom +from machine import Timer + +class Buzzer: + + """ + A class to control the buzzer on XRP robots. + + Allows for playing individual notes or songs with various durations and tempos. + Supports natural notes, sharps, and flats. + """ + + # Pin configuration for the buzzer + BUZZER_PIN = 13 + + # Duration constants (based on a default 120 BPM tempo) + WHOLE = 4 + HALF = 2 + QUARTER = 1 + EIGHTH = 0.5 + SIXTEENTH = 0.25 + + # Note names to semitone offset (semitones from C4 = 0) + NOTE_OFFSETS = { + # Natural notes + "C": 0, "D": 2, "E": 4, "F": 5, "G": 7, "A": 9, "B": 11, + # Sharps + "C#": 1, "D#": 3, "F#": 6, "G#": 8, "A#": 10, + # Flats + "DB": 1, "EB": 3, "GB": 6, "AB": 8, "BB": 10, + } + + _DEFAULT_BUZZER_INSTANCE = None + + @classmethod + def get_default_buzzer(cls): + """ + Get the default XRP buzzer instance. This is a singleton. + + :returns: The default Buzzer instance + :rtype: Buzzer + """ + if cls._DEFAULT_BUZZER_INSTANCE is None: + cls._DEFAULT_BUZZER_INSTANCE = cls() + return cls._DEFAULT_BUZZER_INSTANCE + + def __init__(self, pin: int = None): + """ + Initialize the buzzer. + + :param pin: The pin number for the buzzer + :type pin: int + """ + if pin is None: + pin = self.BUZZER_PIN + self._pin = machine.Pin(pin) + self._pwm = machine.PWM(self._pin) + self._pwm.duty_u16(0) # Start silent + + # State variables for non-blocking playback + self._note_end_time = 0 + self._tempo = 120 # Default BPM + + # Song playback state + self._song = [] + self._song_index = 0 + self._song_tempo = 120 + + # Timer for non-blocking updates (use virtual timer -1 to not conflict with user timers) + self._timer = Timer(-1) + self._timer_in_use = False + + def _compute_frequency(self, note: str, octave: int) -> int: + """ + Compute the frequency for a note at a given octave. + + Uses the formula: f = 440 * 2^((n-69)/12) + """ + if note == "R" or note == "REST": + return 0 + + note_offset = self.NOTE_OFFSETS.get(note.upper(), 0) + midi_note = 60 + (octave - 4) * 12 + note_offset + frequency = int(440 * (2 ** ((midi_note - 69) / 12))) + + return frequency + + def _duration_to_ms(self, duration, tempo: int = None) -> int: + """ + Convert a duration name to milliseconds. + """ + if tempo is None: + tempo = self._tempo + + if isinstance(duration, str): + duration_lower = duration.lower() + if duration_lower == "whole": + beats = 4 + elif duration_lower == "half": + beats = 2 + elif duration_lower == "quarter": + beats = 1 + elif duration_lower == "eighth": + beats = 0.5 + elif duration_lower == "sixteenth": + beats = 0.25 + else: + beats = 1 + else: + beats = duration + + ms_per_beat = 60000 / tempo + return int(beats * ms_per_beat) + + def _parse_note(self, note_str: str): + """Parse a note string like "C4", "C#4", "Db4" or "rest".""" + note_str = note_str.strip().upper() + + if note_str == "REST" or note_str == "R": + return ("R", 0) + + note_letter = note_str[0] + if note_letter in "ABCDEFG": + # Check for sharp (#) or flat (b) modifier + note_with_modifier = note_letter + if len(note_str) > 1: + if note_str[1] == "#": + note_with_modifier = note_letter + "#" + octave_pos = 2 + elif note_str[1] == "B": + note_with_modifier = note_letter + "B" + octave_pos = 2 + else: + octave_pos = 1 + + try: + if octave_pos < len(note_str): + octave = int(note_str[octave_pos]) + else: + octave = 4 # Default octave + return (note_with_modifier, octave) + except (ValueError, IndexError): + return None + + return None + + def _update(self, timer): + """ + Internal callback for non-blocking playback. + Called by the timer when running in non-blocking mode. + """ + + # Check if current note is done + if time.ticks_diff(self._note_end_time, time.ticks_ms()) <= 0: + # Note done - stop the tone + self._pwm.duty_u16(0) + + # Small gap between notes + time.sleep_ms(20) + + # Play next note in song + self._song_index += 1 + if self._song_index < len(self._song): + note, duration = self._song[self._song_index] + self._start_note(note, duration, self._song_tempo) + else: + # Song finished + self._timer.deinit() + self._timer_in_use = False + + def _start_note(self, note: str, duration: str = "quarter", tempo: int = None): + """Start playing a note (non-blocking helper).""" + parsed = self._parse_note(note) + if parsed is None: + return + + note_letter, octave = parsed + + if note_letter == "R": + frequency = 0 + else: + frequency = self._compute_frequency(note_letter, octave) + + duration_ms = self._duration_to_ms(duration, tempo) + + if frequency > 0: + self._pwm.freq(frequency) + self._pwm.duty_u16(32768) # 50% duty cycle + + self._note_end_time = time.ticks_add(time.ticks_ms(), duration_ms) + + def _play_tone_blocking(self, frequency: int, duration_ms: int): + """Play a tone synchronously (blocking).""" + if frequency <= 0: + self._pwm.duty_u16(0) + else: + self._pwm.freq(frequency) + self._pwm.duty_u16(32768) + + time.sleep_ms(duration_ms) + self._pwm.duty_u16(0) + time.sleep_ms(20) + + def play_note(self, note: str, duration: str = "quarter", blocking: bool = True, tempo: int = None): + """ + Play a single note on the buzzer. + + :param note: The note to play (e.g., "C4", "G5", "C#4", "Db4", "rest") + :type note: str + :param duration: How long to play ("whole", "half", "quarter", "eighth", "sixteenth") + :type duration: str + :param blocking: If True, wait for note to finish. If False, return immediately. + :type blocking: bool + :param tempo: BPM (optional, uses default if not specified) + :type tempo: int + """ + parsed = self._parse_note(note) + if parsed is None: + return + + note_letter, octave = parsed + + if note_letter == "R": + frequency = 0 + else: + frequency = self._compute_frequency(note_letter, octave) + + duration_ms = self._duration_to_ms(duration, tempo) + + if blocking: + self._play_tone_blocking(frequency, duration_ms) + else: + # Non-blocking - start the note and set up timer if needed + if frequency > 0: + self._pwm.freq(frequency) + self._pwm.duty_u16(32768) + + self._note_end_time = time.ticks_add(time.ticks_ms(), duration_ms) + + # Start timer if not already running + if not self._timer_in_use: + self._timer.init(freq=100, callback=lambda t: self._update(t)) + self._timer_in_use = True + + def set_tempo(self, bpm: int): + """ + Set the tempo for songs. + + :param bpm: The tempo in beats per minute + :type bpm: int + """ + self._tempo = bpm + + def play_song(self, song: list, tempo: int = None, blocking: bool = True): + """ + Play a song from a list of notes. + + :param song: A list of (note, duration) tuples + :type song: list + :param tempo: BPM for this song (optional, uses default) + :type tempo: int + :param blocking: If True, wait for song to finish. If False, play in background. + :type blocking: bool + """ + + if tempo is None: + tempo = self._tempo + + if not song: + return + + if blocking: + # Play all notes synchronously + for note, duration in song: + parsed = self._parse_note(note) + if parsed is None: + continue + note_letter, octave = parsed + if note_letter == "R": + frequency = 0 + else: + frequency = self._compute_frequency(note_letter, octave) + duration_ms = self._duration_to_ms(duration, tempo) + self._play_tone_blocking(frequency, duration_ms) + else: + # Non-blocking - store song and start playing with timer + self._song = song + self._song_index = 0 + self._song_tempo = tempo + self._timer_in_use = False + + # Start playing first note + note, duration = song[0] + self._start_note(note, duration, tempo) + self._song_index = 1 + + # Start the timer for non-blocking playback + self._timer.init(freq=100, callback=lambda t: self._update(t)) + self._timer_in_use = True + + def play_move_it(self, blocking: bool = True): + """ + Play "I Like to Move It" song. + + :param blocking: If True, wait for song to finish. If False, return immediately. + :type blocking: bool + """ + tempo = 120 + move_it = [ + ("rest", "eighth"), + ("C4", "eighth"), ("C4", "eighth"), ("C4", "eighth"), ("C4", "eighth"), + ("C4", "eighth"), ("C4", "eighth"), ("G3", "eighth"), + ("rest", "eighth"), + ("C4", "eighth"), ("C4", "eighth"), ("C4", "eighth"), ("C4", "eighth"), + ("C4", "eighth"), ("C4", "eighth"), ("G3", "eighth"), + ("rest", "eighth"), + ("C4", "eighth"), ("C4", "eighth"), ("C4", "eighth"), ("C4", "eighth"), + ("C4", "eighth"), ("C4", "eighth"), ("G3", "eighth"), + ("rest", "eighth"), + ("C4", "eighth"), ("C4", "eighth"), ("C4", "eighth"), + ("rest", "eighth"), ("rest", "eighth"), + ("G4", "eighth"), ("G4", "eighth"), + ] + self.play_song(move_it, tempo, blocking) + + def play_happy_birthday(self, blocking: bool = True): + """ + Play "Happy Birthday" song. + + :param blocking: If True, wait for song to finish. If False, return immediately. + :type blocking: bool + """ + tempo = 140 + happy_birthday = [ + ("G4", "quarter"), ("G4", "quarter"), ("A4", "half"), ("G4", "half"), + ("C5", "half"), ("B4", "whole"), + ("G4", "quarter"), ("G4", "quarter"), ("A4", "half"), ("G4", "half"), + ("D5", "half"), ("C5", "whole"), + ("G4", "quarter"), ("G4", "quarter"), ("G5", "half"), ("C5", "half"), + ("B4", "quarter"), ("A4", "quarter"), ("F5", "half"), ("E5", "half"), + ("C5", "half"), ("D5", "half"), ("C5", "whole"), + ] + self.play_song(happy_birthday, tempo, blocking) + + def play_twinkle_twinkle(self, blocking: bool = True): + """ + Play "Twinkle Twinkle Little Star" song. + + :param blocking: If True, wait for song to finish. If False, return immediately. + :type blocking: bool + """ + tempo = 100 + twinkle_twinkle = [ + ("C4", "quarter"), ("C4", "quarter"), ("G4", "quarter"), ("G4", "quarter"), + ("A4", "quarter"), ("A4", "quarter"), ("G4", "half"), + ("F4", "quarter"), ("F4", "quarter"), ("E4", "quarter"), ("E4", "quarter"), + ("D4", "quarter"), ("D4", "quarter"), ("C4", "half"), + ("G4", "quarter"), ("G4", "quarter"), ("F4", "quarter"), ("F4", "quarter"), + ("E4", "quarter"), ("E4", "quarter"), ("D4", "half"), + ("G4", "quarter"), ("G4", "quarter"), ("F4", "quarter"), ("F4", "quarter"), + ("E4", "quarter"), ("E4", "quarter"), ("D4", "half"), + ("C4", "quarter"), ("C4", "quarter"), ("G4", "quarter"), ("G4", "quarter"), + ("A4", "quarter"), ("A4", "quarter"), ("G4", "half"), + ("F4", "quarter"), ("F4", "quarter"), ("E4", "quarter"), ("E4", "quarter"), + ("D4", "quarter"), ("D4", "quarter"), ("C4", "half"), + ] + self.play_song(twinkle_twinkle, tempo, blocking) \ No newline at end of file diff --git a/XRPLib/defaults.py b/XRPLib/defaults.py index 52c474d..a078346 100644 --- a/XRPLib/defaults.py +++ b/XRPLib/defaults.py @@ -8,6 +8,7 @@ from .reflectance import Reflectance from .servo import Servo from .webserver import Webserver +from .buzzer import Buzzer from machine import Pin from sys import implementation @@ -39,4 +40,7 @@ if hasattr(Pin.board, "SERVO_3"): servo_three = Servo.get_default_servo(index=3) if hasattr(Pin.board, "SERVO_4"): - servo_four = Servo.get_default_servo(index=4) \ No newline at end of file + servo_four = Servo.get_default_servo(index=4) + +if "NanoXRP" in implementation._machine: + buzzer = Buzzer.get_default_buzzer() From c46f121ddc29850faee21de0dd2cec75f79c2a34 Mon Sep 17 00:00:00 2001 From: Jacob Williams Date: Sat, 4 Apr 2026 02:21:43 -0400 Subject: [PATCH 11/11] Fix gyro axis mapping for NanoXRP Move gyro delta calculations into the NanoXRP branch and correctly swap pitch/roll while inverting yaw. Previously the deltas were computed before the platform check and only yaw was negated, which produced incorrect pitch/roll mapping on NanoXRP devices. The original computation is preserved for other implementations. --- XRPLib/imu.py | 11 +++++++---- 1 file changed, 7 insertions(+), 4 deletions(-) diff --git a/XRPLib/imu.py b/XRPLib/imu.py index e865f73..c935c5a 100644 --- a/XRPLib/imu.py +++ b/XRPLib/imu.py @@ -549,13 +549,16 @@ def _stop_timer(self): def _update_imu_readings(self): # Called every tick through a callback timer self.get_gyro_rates() - delta_pitch = self.irq_v[1][0] / 1000 / self.timer_frequency - delta_roll = self.irq_v[1][1] / 1000 / self.timer_frequency - delta_yaw = self.irq_v[1][2] / 1000 / self.timer_frequency # Flip and swap for NanoXRP if "NanoXRP" in implementation._machine: - delta_yaw = -delta_yaw + delta_pitch = self.irq_v[1][1] / 1000 / self.timer_frequency + delta_roll = self.irq_v[1][0] / 1000 / self.timer_frequency + delta_yaw = -1 * self.irq_v[1][2] / 1000 / self.timer_frequency + else: + delta_pitch = self.irq_v[1][0] / 1000 / self.timer_frequency + delta_roll = self.irq_v[1][1] / 1000 / self.timer_frequency + delta_yaw = self.irq_v[1][2] / 1000 / self.timer_frequency state = disable_irq() self.running_pitch += delta_pitch