Intuitive Switching between motor types

Author: ksiegall

XRPLib/board.py

@@ -113,4 +113,4 @@ def set_rgb_led(self, r:int, g:int, b:int):
             self.rgb_led[0] = (r, g, b)
             self.rgb_led.write()
         else:
-            raise Exception("No RGB LED found for this board")
+            raise NotImplementedError("Board.set_rgb_led not implemented for the XRP RP2040")

XRPLib/encoded_motor.py

@@ -1,4 +1,4 @@
-from .motor import Motor
+from .motor import Motor, SinglePWMMotor, DualPWMMotor
 from .encoder import Encoder
 from machine import Timer
 from .controller import Controller
@@ -21,35 +21,37 @@ def get_default_encoded_motor(cls, index:int = 1):
         :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:
-            usePwm = True
+            MotorImplementation = DualPWMMotor
         else:
-            usePwm = False
+            MotorImplementation = SinglePWMMotor
+
         if index == 1:
             if cls._DEFAULT_LEFT_MOTOR_INSTANCE is None:
                 cls._DEFAULT_LEFT_MOTOR_INSTANCE = cls(
-                    Motor("MOTOR_L_IN1", "MOTOR_L_IN2", flip_dir=True, pwmMode=usePwm),
+                    MotorImplementation("MOTOR_L_IN1", "MOTOR_L_IN2", flip_dir=True),
                     Encoder(0, "ENCODER_LA", "ENCODER_LB")
                 )
             motor = cls._DEFAULT_LEFT_MOTOR_INSTANCE
         elif index == 2:
             if cls._DEFAULT_RIGHT_MOTOR_INSTANCE is None:
                 cls._DEFAULT_RIGHT_MOTOR_INSTANCE = cls(
-                    Motor("MOTOR_R_IN1", "MOTOR_R_IN2", pwmMode=usePwm),
+                    MotorImplementation("MOTOR_R_IN1", "MOTOR_R_IN2"),
                     Encoder(1, "ENCODER_RA", "ENCODER_RB")
                 )
             motor = cls._DEFAULT_RIGHT_MOTOR_INSTANCE
         elif index == 3:
             if cls._DEFAULT_MOTOR_THREE_INSTANCE is None:
                 cls._DEFAULT_MOTOR_THREE_INSTANCE = cls(
-                    Motor("MOTOR_3_IN1", "MOTOR_3_IN2", flip_dir=True, pwmMode=usePwm),
+                    MotorImplementation("MOTOR_3_IN1", "MOTOR_3_IN2", flip_dir=True),
                     Encoder(2, "ENCODER_3A", "ENCODER_3B")
                 )
             motor = cls._DEFAULT_MOTOR_THREE_INSTANCE
         elif index == 4:
             if cls._DEFAULT_MOTOR_FOUR_INSTANCE is None:
                 cls._DEFAULT_MOTOR_FOUR_INSTANCE = cls(
-                    Motor("MOTOR_4_IN1", "MOTOR_4_IN2", pwmMode=usePwm),
+                    MotorImplementation("MOTOR_4_IN1", "MOTOR_4_IN2"),
                     Encoder(3, "ENCODER_4A", "ENCODER_4B")
                 )
             motor = cls._DEFAULT_MOTOR_FOUR_INSTANCE

XRPLib/motor.py

@@ -1,24 +1,20 @@
 from machine import Pin, PWM
-class Motor:
+
+class SinglePWMMotor:
 
     """
     A wrapper class handling direction and power sets for DC motors on the XRP robots
+
+    This version is used for the beta version of the XRP, which uses the rp2040 processor
     """
 
-    def __init__(self, in1_direction_pin: int, in2_speed_pin: int, flip_dir:bool=False, pwmMode:bool=False):
-        self._pwmMode = pwmMode
+    def __init__(self, in1_direction_pin: int, in2_speed_pin: int, flip_dir:bool=False):
         self.flip_dir = flip_dir
         self._MAX_PWM = 65534 # Motor holds when actually at full power
 
-        if self._pwmMode:
-            self._in1DirPin = PWM(Pin(in1_direction_pin, Pin.OUT))
-            self._in2SpeedPin = PWM(Pin(in2_speed_pin, Pin.OUT))
-            self._in1DirPin.freq(50)
-            self._in2SpeedPin.freq(50)
-        else:
-            self._in1DirPin = Pin(in1_direction_pin, Pin.OUT)
-            self._in2SpeedPin = PWM(Pin(in2_speed_pin, Pin.OUT))
-            self._in2SpeedPin.freq(50)
+        self._in1DirPin = Pin(in1_direction_pin, Pin.OUT)
+        self._in2SpeedPin = PWM(Pin(in2_speed_pin, Pin.OUT))
+        self._in2SpeedPin.freq(50)
 
     def set_effort(self, effort: float):
         """
@@ -28,28 +24,73 @@ def set_effort(self, effort: float):
         :type effort: float
         """
 
-        if self._pwmMode:
-            in1Pwm = (effort < 0) ^ (self.flip_dir)
-            if in1Pwm:
-                self._in1DirPin.duty_u16(int(abs(effort)*self._MAX_PWM))
-                self._in2SpeedPin.duty_u16(int(0))
-            else:
-                self._in1DirPin.duty_u16(int(0))
-                self._in2SpeedPin.duty_u16(int(abs(effort)*self._MAX_PWM))
+        if effort < 0:
+            # Change direction if negative power
+            effort *= -1
+            self._set_direction(1)
         else:
-            if effort < 0:
-                # Change direction if negative power
-                effort *= -1
-                self._set_direction(1)
-            else:
-                self._set_direction(0)
-            # Cap power to [0,1]
-            effort = max(0,min(effort,1))
-            self._in2SpeedPin.duty_u16(int(effort*self._MAX_PWM))
-                
+            self._set_direction(0)
+        # Cap power to [0,1]
+        effort = max(0,min(effort,1))
+        self._in2SpeedPin.duty_u16(int(effort*self._MAX_PWM))
 
     def _set_direction(self, direction: int):
         if self.flip_dir:
             self._in1DirPin.value(not direction)
         else:
             self._in1DirPin.value(direction)
+
+    def brake(self):
+        # Motor holds with the real max duty cycle (65535)
+        self._in2SpeedPin.duty_u16(self._MAX_PWM+1)
+
+    def coast(self):
+        raise NotImplementedError("Motor.coast is not implemented for not implemented for the XRP RP2040")
+
+class DualPWMMotor:
+    """
+    A wrapper class handling direction and power sets for DC motors on the XRP robots
+
+    This version of the Motor class is used 
+    """
+
+    def __init__(self, in1_pwm_forward: int, in2_pwm_backward: int, flip_dir:bool=False):
+        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))
+        self._in2BackwardPin = PWM(Pin(in2_pwm_backward, Pin.OUT))
+        self._in1ForwardPin.freq(50)
+        self._in2BackwardPin.freq(50)
+
+    def set_effort(self, effort: float):
+        """
+        Sets the effort value of the motor (corresponds to power)
+
+        :param effort: The effort to set the motor to, between -1 and 1
+        :type effort: float
+        """
+
+        in1Pwm = (effort < 0) ^ (self.flip_dir)
+        if in1Pwm:
+            self._in1ForwardPin.duty_u16(int(abs(effort)*self._MAX_PWM))
+            self._in2BackwardPin.duty_u16(int(0))
+        else:
+            self._in1ForwardPin.duty_u16(int(0))
+            self._in2BackwardPin.duty_u16(int(abs(effort)*self._MAX_PWM))
+
+    def brake(self):
+        """
+        Powers the motor in both directions at the same time, enabling it to hold position
+        """
+        self._in1ForwardPin.duty_u16(int(self._MAX_PWM))
+        self._in2BackwardPin.duty_u16(int(self._MAX_PWM))
+
+    def coast(self):
+        """
+        Disables the motor in both directions at the same time, enabling it to spin freely
+        """
+        self._in1ForwardPin.duty_u16(int(0))
+        self._in2BackwardPin.duty_u16(int(0))
+        
+