Skip to content

Commit 3495695

Browse files
ksiegallksiegall
committed
Intuitive Switching between motor types
1 parent 3364532 commit 3495695

File tree

3 files changed

+81
-38
lines changed

3 files changed

+81
-38
lines changed

‎XRPLib/board.py

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -113,4 +113,4 @@ def set_rgb_led(self, r:int, g:int, b:int):
113113
self.rgb_led[0] = (r, g, b)
114114
self.rgb_led.write()
115115
else:
116-
raise Exception("No RGB LED found for this board")
116+
raise NotImplementedError("Board.set_rgb_led not implemented for the XRP RP2040")

‎XRPLib/encoded_motor.py

Lines changed: 9 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -1,4 +1,4 @@
1-
from .motor import Motor
1+
from .motor import Motor, SinglePWMMotor, DualPWMMotor
22
from .encoder import Encoder
33
from machine import Timer
44
from .controller import Controller
@@ -21,35 +21,37 @@ def get_default_encoded_motor(cls, index:int = 1):
2121
:param index: The index of the motor to get; 1 for left, 2 for right, 3 for motor 3, 4 for motor 4
2222
:type index: int
2323
"""
24+
2425
if "RP2350" in sys.implementation._machine:
25-
usePwm = True
26+
MotorImplementation = DualPWMMotor
2627
else:
27-
usePwm = False
28+
MotorImplementation = SinglePWMMotor
29+
2830
if index == 1:
2931
if cls._DEFAULT_LEFT_MOTOR_INSTANCE is None:
3032
cls._DEFAULT_LEFT_MOTOR_INSTANCE = cls(
31-
Motor("MOTOR_L_IN1", "MOTOR_L_IN2", flip_dir=True, pwmMode=usePwm),
33+
MotorImplementation("MOTOR_L_IN1", "MOTOR_L_IN2", flip_dir=True),
3234
Encoder(0, "ENCODER_LA", "ENCODER_LB")
3335
)
3436
motor = cls._DEFAULT_LEFT_MOTOR_INSTANCE
3537
elif index == 2:
3638
if cls._DEFAULT_RIGHT_MOTOR_INSTANCE is None:
3739
cls._DEFAULT_RIGHT_MOTOR_INSTANCE = cls(
38-
Motor("MOTOR_R_IN1", "MOTOR_R_IN2", pwmMode=usePwm),
40+
MotorImplementation("MOTOR_R_IN1", "MOTOR_R_IN2"),
3941
Encoder(1, "ENCODER_RA", "ENCODER_RB")
4042
)
4143
motor = cls._DEFAULT_RIGHT_MOTOR_INSTANCE
4244
elif index == 3:
4345
if cls._DEFAULT_MOTOR_THREE_INSTANCE is None:
4446
cls._DEFAULT_MOTOR_THREE_INSTANCE = cls(
45-
Motor("MOTOR_3_IN1", "MOTOR_3_IN2", flip_dir=True, pwmMode=usePwm),
47+
MotorImplementation("MOTOR_3_IN1", "MOTOR_3_IN2", flip_dir=True),
4648
Encoder(2, "ENCODER_3A", "ENCODER_3B")
4749
)
4850
motor = cls._DEFAULT_MOTOR_THREE_INSTANCE
4951
elif index == 4:
5052
if cls._DEFAULT_MOTOR_FOUR_INSTANCE is None:
5153
cls._DEFAULT_MOTOR_FOUR_INSTANCE = cls(
52-
Motor("MOTOR_4_IN1", "MOTOR_4_IN2", pwmMode=usePwm),
54+
MotorImplementation("MOTOR_4_IN1", "MOTOR_4_IN2"),
5355
Encoder(3, "ENCODER_4A", "ENCODER_4B")
5456
)
5557
motor = cls._DEFAULT_MOTOR_FOUR_INSTANCE

‎XRPLib/motor.py

Lines changed: 71 additions & 30 deletions
Original file line numberDiff line numberDiff line change
@@ -1,24 +1,20 @@
11
from machine import Pin, PWM
2-
class Motor:
2+
3+
class SinglePWMMotor:
34

45
"""
56
A wrapper class handling direction and power sets for DC motors on the XRP robots
7+
8+
This version is used for the beta version of the XRP, which uses the rp2040 processor
69
"""
710

8-
def __init__(self, in1_direction_pin: int, in2_speed_pin: int, flip_dir:bool=False, pwmMode:bool=False):
9-
self._pwmMode = pwmMode
11+
def __init__(self, in1_direction_pin: int, in2_speed_pin: int, flip_dir:bool=False):
1012
self.flip_dir = flip_dir
1113
self._MAX_PWM = 65534 # Motor holds when actually at full power
1214

13-
if self._pwmMode:
14-
self._in1DirPin = PWM(Pin(in1_direction_pin, Pin.OUT))
15-
self._in2SpeedPin = PWM(Pin(in2_speed_pin, Pin.OUT))
16-
self._in1DirPin.freq(50)
17-
self._in2SpeedPin.freq(50)
18-
else:
19-
self._in1DirPin = Pin(in1_direction_pin, Pin.OUT)
20-
self._in2SpeedPin = PWM(Pin(in2_speed_pin, Pin.OUT))
21-
self._in2SpeedPin.freq(50)
15+
self._in1DirPin = Pin(in1_direction_pin, Pin.OUT)
16+
self._in2SpeedPin = PWM(Pin(in2_speed_pin, Pin.OUT))
17+
self._in2SpeedPin.freq(50)
2218

2319
def set_effort(self, effort: float):
2420
"""
@@ -28,28 +24,73 @@ def set_effort(self, effort: float):
2824
:type effort: float
2925
"""
3026

31-
if self._pwmMode:
32-
in1Pwm = (effort < 0) ^ (self.flip_dir)
33-
if in1Pwm:
34-
self._in1DirPin.duty_u16(int(abs(effort)*self._MAX_PWM))
35-
self._in2SpeedPin.duty_u16(int(0))
36-
else:
37-
self._in1DirPin.duty_u16(int(0))
38-
self._in2SpeedPin.duty_u16(int(abs(effort)*self._MAX_PWM))
27+
if effort < 0:
28+
# Change direction if negative power
29+
effort *= -1
30+
self._set_direction(1)
3931
else:
40-
if effort < 0:
41-
# Change direction if negative power
42-
effort *= -1
43-
self._set_direction(1)
44-
else:
45-
self._set_direction(0)
46-
# Cap power to [0,1]
47-
effort = max(0,min(effort,1))
48-
self._in2SpeedPin.duty_u16(int(effort*self._MAX_PWM))
49-
32+
self._set_direction(0)
33+
# Cap power to [0,1]
34+
effort = max(0,min(effort,1))
35+
self._in2SpeedPin.duty_u16(int(effort*self._MAX_PWM))
5036

5137
def _set_direction(self, direction: int):
5238
if self.flip_dir:
5339
self._in1DirPin.value(not direction)
5440
else:
5541
self._in1DirPin.value(direction)
42+
43+
def brake(self):
44+
# Motor holds with the real max duty cycle (65535)
45+
self._in2SpeedPin.duty_u16(self._MAX_PWM+1)
46+
47+
def coast(self):
48+
raise NotImplementedError("Motor.coast is not implemented for not implemented for the XRP RP2040")
49+
50+
class DualPWMMotor:
51+
"""
52+
A wrapper class handling direction and power sets for DC motors on the XRP robots
53+
54+
This version of the Motor class is used
55+
"""
56+
57+
def __init__(self, in1_pwm_forward: int, in2_pwm_backward: int, flip_dir:bool=False):
58+
self.flip_dir = flip_dir
59+
self._MAX_PWM = 65535 # Motor holds when actually at full power
60+
61+
self._in1ForwardPin = PWM(Pin(in1_pwm_forward, Pin.OUT))
62+
self._in2BackwardPin = PWM(Pin(in2_pwm_backward, Pin.OUT))
63+
self._in1ForwardPin.freq(50)
64+
self._in2BackwardPin.freq(50)
65+
66+
def set_effort(self, effort: float):
67+
"""
68+
Sets the effort value of the motor (corresponds to power)
69+
70+
:param effort: The effort to set the motor to, between -1 and 1
71+
:type effort: float
72+
"""
73+
74+
in1Pwm = (effort < 0) ^ (self.flip_dir)
75+
if in1Pwm:
76+
self._in1ForwardPin.duty_u16(int(abs(effort)*self._MAX_PWM))
77+
self._in2BackwardPin.duty_u16(int(0))
78+
else:
79+
self._in1ForwardPin.duty_u16(int(0))
80+
self._in2BackwardPin.duty_u16(int(abs(effort)*self._MAX_PWM))
81+
82+
def brake(self):
83+
"""
84+
Powers the motor in both directions at the same time, enabling it to hold position
85+
"""
86+
self._in1ForwardPin.duty_u16(int(self._MAX_PWM))
87+
self._in2BackwardPin.duty_u16(int(self._MAX_PWM))
88+
89+
def coast(self):
90+
"""
91+
Disables the motor in both directions at the same time, enabling it to spin freely
92+
"""
93+
self._in1ForwardPin.duty_u16(int(0))
94+
self._in2BackwardPin.duty_u16(int(0))
95+
96+

0 commit comments

Comments
 (0)