Merge pull request #76 from Open-STEM/OfficialRelease

Moving out of Beta

Author: ksiegall

.DS_Store

@@ -5,4 +5,5 @@ docs/_build/
 .picowgo
 secrets.json
 venv
-.temp_xrplib
+.temp_xrplib
+XRPLib/__pycache__

.gitignore

@@ -1,5 +1,7 @@
 from machine import Pin, ADC, Timer
+from neopixel import NeoPixel
 import time
+import sys
 
 class Board:
 
@@ -11,12 +13,13 @@ def get_default_board(cls):
         Get the default board instance. This is a singleton, so only one instance of the board will ever exist.
         """
         if cls._DEFAULT_BOARD_INSTANCE is None:
-            cls._DEFAULT_BOARD_INSTANCE = cls(28,22)
+            cls._DEFAULT_BOARD_INSTANCE = cls()
         return cls._DEFAULT_BOARD_INSTANCE
 
-    def __init__(self, vin_pin:int, button_pin:int):
+    def __init__(self, vin_pin="BOARD_VIN_MEASURE", button_pin="BOARD_USER_BUTTON", 
+                 rgb_led_pin = "BOARD_NEOPIXEL", led_pin = "LED"):
         """
-        Implements for extra features on the XRP v2 board. Handles the on/off switch, button, and LED.
+        Implements for extra features on the XRP board. Handles the on/off switch, button, and LED.
 
         :param vin_pin: The pin the on/off switch is connected to
         :type vin_pin: int
@@ -28,7 +31,10 @@ def __init__(self, vin_pin:int, button_pin:int):
 
         self.button = Pin(button_pin, Pin.IN, Pin.PULL_UP)
 
-        self.led = Pin("LED", Pin.OUT)
+        self.led = Pin(led_pin, Pin.OUT)
+
+        if "RP2350" in sys.implementation._machine:
+            self.rgb_led = NeoPixel(Pin(rgb_led_pin, Pin.OUT), 1)
         # A timer ID of -1 is a virtual timer.
         # Leaves the hardware timers for more important uses
         self._virt_timer = Timer(-1)
@@ -102,3 +108,10 @@ def led_blink(self, frequency: int=0):
         else:
             self._virt_timer.deinit()
             self.is_led_blinking = False
+
+    def set_rgb_led(self, r:int, g:int, b:int):
+        if "rgb_led" in self.__dict__:
+            self.rgb_led[0] = (r, g, b)
+            self.rgb_led.write()
+        else:
+            raise NotImplementedError("Board.set_rgb_led not implemented for the XRP Beta")

XRPLib/board.py

@@ -1,6 +1,6 @@
 from .board import Board
 from .differential_drive import DifferentialDrive
-from .motor import Motor
+from .motor import SinglePWMMotor, DualPWMMotor
 from .encoder import Encoder
 from .encoded_motor import EncodedMotor
 from .rangefinder import Rangefinder
@@ -14,8 +14,8 @@
 Run "from XRPLib.defaults import *" to use
 """
 
-left_motor = EncodedMotor.get_default_encoded_motor(index=1)
-right_motor = EncodedMotor.get_default_encoded_motor(index=2)
+left_motor = EncodedMotor.get_default_encoded_motor(index=3)
+right_motor = EncodedMotor.get_default_encoded_motor(index=4)
 imu = IMU.get_default_imu()
 drivetrain = DifferentialDrive.get_default_differential_drive()
 rangefinder = Rangefinder.get_default_rangefinder()

XRPLib/defaults.py

@@ -14,7 +14,7 @@ class DifferentialDrive:
     def get_default_differential_drive(cls):
 
         """
-        Get the default XRP v2 differential drive instance. This is a singleton, so only one instance of the drivetrain will ever exist.
+        Get the default XRP differential drive instance. This is a singleton, so only one instance of the drivetrain will ever exist.
         """
 
         if cls._DEFAULT_DIFFERENTIAL_DRIVE_INSTANCE is None:
@@ -46,6 +46,7 @@ def __init__(self, left_motor: EncodedMotor, right_motor: EncodedMotor, imu: IMU
         self.right_motor = right_motor
         self.imu = imu
 
+        self.brake_at_zero_power = False
         self.wheel_diam = wheel_diam
         self.track_width = wheel_track
 
@@ -76,9 +77,19 @@ def set_speed(self, left_speed: float, right_speed: float) -> None:
         self.left_motor.set_speed(left_speed*cmpsToRPM)
         self.right_motor.set_speed(right_speed*cmpsToRPM)
 
+    def set_zero_effort_behavior(self, brake_at_zero_effort):
+
+        """
+        Sets the behavior of both motor at 0 effort to either brake (hold position) or coast (free spin)
+        :param brake_at_zero_effort: Whether or not to brake at 0 effort. Can use EncodedMotor.ZERO_EFFORT_BREAK or EncodedMotor.ZERO_EFFORT_COAST for clarity.
+        :type brake_at_zero_effort: bool
+        """
+        self.left_motor.set_zero_effort_behavior(brake_at_zero_effort)
+        self.right_motor.set_zero_effort_behavior(brake_at_zero_effort)
+
     def stop(self) -> None:
         """
-        Stops both drivetrain motors
+        Stops both drivetrain motors by setting power to zero.
         """
         self.left_motor.set_speed()
         self.right_motor.set_speed()

XRPLib/differential_drive.py

@@ -1,11 +1,15 @@
-from .motor import Motor
+from .motor import SinglePWMMotor, DualPWMMotor
 from .encoder import Encoder
 from machine import Timer
 from .controller import Controller
 from .pid import PID
+import sys
 
 class EncodedMotor:
 
+    ZERO_EFFORT_BREAK = True
+    ZERO_EFFORT_COAST = False
+
     _DEFAULT_LEFT_MOTOR_INSTANCE = None
     _DEFAULT_RIGHT_MOTOR_INSTANCE = None
     _DEFAULT_MOTOR_THREE_INSTANCE = None
@@ -14,49 +18,57 @@ class EncodedMotor:
     @classmethod
     def get_default_encoded_motor(cls, index:int = 1):
         """
-        Get one of the default XRP v2 motor instances. These are singletons, so only one instance of each of these will ever exist.
+        Get one of the default XRP motor instances. These are singletons, so only one instance of each of these will ever exist.
         Raises an exception if an invalid index is requested.
 
         :param index: The index of the motor to get; 1 for left, 2 for right, 3 for motor 3, 4 for motor 4
         :type index: int
         """
+        
+        if "RP2350" in sys.implementation._machine:
+            MotorImplementation = DualPWMMotor
+        else:
+            MotorImplementation = SinglePWMMotor
+
         if index == 1:
             if cls._DEFAULT_LEFT_MOTOR_INSTANCE is None:
                 cls._DEFAULT_LEFT_MOTOR_INSTANCE = cls(
-                    Motor(6, 7, flip_dir=True),
-                    Encoder(0, 4, 5)
+                    MotorImplementation("MOTOR_L_IN_1", "MOTOR_L_IN_2", flip_dir=True),
+                    Encoder(0, "MOTOR_L_ENCODER_A", "MOTOR_L_ENCODER_B")
                 )
             motor = cls._DEFAULT_LEFT_MOTOR_INSTANCE
         elif index == 2:
             if cls._DEFAULT_RIGHT_MOTOR_INSTANCE is None:
                 cls._DEFAULT_RIGHT_MOTOR_INSTANCE = cls(
-                    Motor(14, 15),
-                    Encoder(1, 12, 13)
+                    MotorImplementation("MOTOR_R_IN_1", "MOTOR_R_IN_2"),
+                    Encoder(1, "MOTOR_R_ENCODER_A", "MOTOR_R_ENCODER_B")
                 )
             motor = cls._DEFAULT_RIGHT_MOTOR_INSTANCE
         elif index == 3:
             if cls._DEFAULT_MOTOR_THREE_INSTANCE is None:
                 cls._DEFAULT_MOTOR_THREE_INSTANCE = cls(
-                    Motor(2, 3),
-                    Encoder(2, 0, 1)
+                    MotorImplementation("MOTOR_3_IN_1", "MOTOR_3_IN_2", flip_dir=True),
+                    Encoder(2, "MOTOR_3_ENCODER_A", "MOTOR_3_ENCODER_B")
                 )
             motor = cls._DEFAULT_MOTOR_THREE_INSTANCE
         elif index == 4:
             if cls._DEFAULT_MOTOR_FOUR_INSTANCE is None:
                 cls._DEFAULT_MOTOR_FOUR_INSTANCE = cls(
-                    Motor(10, 11, flip_dir=True),
-                    Encoder(3, 8, 9)
+                    MotorImplementation("MOTOR_4_IN_1", "MOTOR_4_IN_2"),
+                    Encoder(3, "MOTOR_4_ENCODER_A", "MOTOR_4_ENCODER_B")
                 )
             motor = cls._DEFAULT_MOTOR_FOUR_INSTANCE
         else:
             return Exception("Invalid motor index")
         return motor
 
-    def __init__(self, motor: Motor, encoder: Encoder):
+    def __init__(self, motor, encoder: Encoder):
 
         self._motor = motor
         self._encoder = encoder
 
+        self.brake_at_zero = False
+
         self.target_speed = None
         self.DEFAULT_SPEED_CONTROLLER = PID(
             kp=0.035,
@@ -71,12 +83,38 @@ def __init__(self, motor: Motor, encoder: Encoder):
         # If the update timer is not running, start it at 50 Hz (20ms updates)
         self.updateTimer.init(period=20, callback=lambda t:self._update())
 
+
     def set_effort(self, effort: float):
         """
         :param effort: The effort to set this motor to, from -1 to 1
         :type effort: float
         """
-        self._motor.set_effort(effort)
+        if self.brake_at_zero and effort == 0:
+            self.brake()
+        else:
+            self._motor.set_effort(effort)
+    
+    # EncodedMotor.set_zero_effort_behavior(EncodedMotor.ZERO_POWER_BRAKE)
+    def set_zero_effort_behavior(self, brake_at_zero_effort):
+        """
+        Sets the behavior of the motor at 0 effort to either brake (hold position) or coast (free spin)
+        :param brake_at_zero_effort: Whether or not to brake at 0 effort. Can use EncodedMotor.ZERO_EFFORT_BREAK or EncodedMotor.ZERO_EFFORT_COAST for clarity.
+        :type brake_at_zero_effort: bool
+        """
+        self.brake_at_zero = brake_at_zero_effort
+
+    def brake(self):
+        """
+        Causes the motor to resist rotation.
+        """
+        # Exact impl of brake depends on which board is being used. 
+        self._motor.brake()
+
+    def coast(self):
+        """
+        Allows the motor to spin freely.
+        """
+        self._motor.coast()
 
     def get_position(self) -> float:
         """

XRPLib/encoded_motor.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, encBPin):
+    def __init__(self, index, encAPin: int|str, encBPin: int|str):
         """
         Uses the on board PIO State Machine to keep track of encoder positions. 
         Only 4 encoders can be instantiated this way.
@@ -21,9 +21,10 @@ def __init__(self, index, encAPin, encBPin):
         :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))
+        # 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)
         self.sm = rp2.StateMachine(index, self._encoder, in_base=basePin)
         self.reset_encoder_position()
         self.sm.active(1)

XRPLib/encoder.py

@@ -19,19 +19,15 @@ class IMU():
     @classmethod
     def get_default_imu(cls):
         """
-        Get the default XRP v2 IMU instance. This is a singleton, so only one instance of the drivetrain will ever exist.
+        Get the default XRP IMU instance. This is a singleton, so only one instance of the drivetrain will ever exist.
         """
 
         if cls._DEFAULT_IMU_INSTANCE is None:
-            cls._DEFAULT_IMU_INSTANCE = cls(
-                scl_pin=19,
-                sda_pin=18,
-                addr=LSM_ADDR_PRIMARY
-            )  
+            cls._DEFAULT_IMU_INSTANCE = cls()  
             cls._DEFAULT_IMU_INSTANCE.calibrate()
         return cls._DEFAULT_IMU_INSTANCE
 
-    def __init__(self, scl_pin: int, sda_pin: int, addr):
+    def __init__(self, scl_pin: int|str = "I2C_SCL_1", sda_pin: int|str = "I2C_SDA_1", addr=LSM_ADDR_PRIMARY):
         # I2C values
         self.i2c = I2C(id=1, scl=Pin(scl_pin), sda=Pin(sda_pin), freq=400000)
         self.addr = addr
@@ -61,8 +57,10 @@ def __init__(self, scl_pin: int, sda_pin: int, addr):
             pass
 
         # Reset sensor to clear any previous configuration
+        # reset() also sets the board to the default config
         self.reset()
 
+    def _default_config(self):
         # Enable block data update
         self._set_bdu()
 
@@ -186,10 +184,17 @@ def reset(self, wait_for_reset = True, wait_timeout_ms = 100):
                 # Check if register has returned to default value (0x04)
                 self.reg_ctrl3_c_byte[0] = self._getreg(LSM_REG_CTRL3_C)
                 if self.reg_ctrl3_c_byte[0] == 0x04:
+                    self._default_config()
+                    self._start_timer()
                     return True
             # Timeout occurred
+            # Attempt to set default config anyways
+            self._default_config()
+            self._start_timer()
             return False
         else:
+            self._default_config()
+            self._start_timer()
             return True
 
     def get_acc_x(self):