diff --git a/XRPLib/defaults.py b/XRPLib/defaults.py index 38216c8..52c474d 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,9 +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) -motor_four = EncodedMotor.get_default_encoded_motor(index=4) +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(left_motor, right_motor, imu, wheel_diam=3.46, wheel_track=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) diff --git a/XRPLib/encoded_motor.py b/XRPLib/encoded_motor.py index de2fa6f..b9c9700 100644 --- a/XRPLib/encoded_motor.py +++ b/XRPLib/encoded_motor.py @@ -1,9 +1,9 @@ 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 +from sys import implementation class EncodedMotor: @@ -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 implementation._machine: MotorImplementation = SinglePWMMotor + else: + MotorImplementation = DualPWMMotor if index == 1: if cls._DEFAULT_LEFT_MOTOR_INSTANCE is None: @@ -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/encoder.py b/XRPLib/encoder.py index 414b973..4924254 100644 --- a/XRPLib/encoder.py +++ b/XRPLib/encoder.py @@ -3,13 +3,19 @@ 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): + 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 +27,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 +64,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): 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: """ 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()