Skip to content

Commit a0b792c

Browse files
bessmanbessman
committed
Refactor motor controller
- Add motor.py. - Move servo motor functionality into new Servo class. - Remove stepper motor functionality since firmware side implementation is missing. - Add tests. - Add/rewrite docstrings in Numpydoc style - Enable linting of motor.py.
1 parent a456a0a commit a0b792c

File tree

6 files changed

+109
-88
lines changed

6 files changed

+109
-88
lines changed

‎PSL/motor.py‎

Lines changed: 75 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,75 @@
1+
"""Motor control related classes.
2+
3+
Examples
4+
--------
5+
>>> from PSL.motor import Servo
6+
>>> servo = Servo("SQ1")
7+
>>> servo.angle = 30 # Turn motor to 30 degrees position.
8+
"""
9+
from typing import Union
10+
11+
from PSL.waveform_generator import PWMGenerator
12+
13+
MICROSECONDS = 1e6
14+
15+
16+
class Servo:
17+
"""Control servo motors on SQ1-4.
18+
19+
Parameters
20+
----------
21+
pin : {"SQ1", "SQ2", "SQ3", "SQ4"}
22+
Name of the digital output on which to generate the control signal.
23+
pwm_generator : PSL.waveform_generator.PWMGenerator, optional
24+
PWMGenerator instance with which to generate the control signal.
25+
Created automatically if not specified. When contolling multiple
26+
servos, they should all use the same PWMGenerator instance.
27+
min_angle_pulse : int, optional
28+
Pulse length in microseconds corresponding to the minimum (0 degree)
29+
angle of the servo. The default value is 500.
30+
max_angle_pulse : int, optional
31+
Pulse length in microseconds corresponding to the maximum (180 degree)
32+
angle of the servo. The default value is 2500.
33+
angle_range : int
34+
Range of the servo in degrees. The default value is 180.
35+
frequency : float, optional
36+
Frequency of the control signal in Hz. The default value is 50.
37+
38+
Attributes
39+
----------
40+
angle
41+
"""
42+
43+
def __init__(
44+
self,
45+
pin: str,
46+
pwm_generator: PWMGenerator = None,
47+
min_angle_pulse: int = 500,
48+
max_angle_pulse: int = 2500,
49+
angle_range: int = 180,
50+
frequency: float = 50,
51+
):
52+
self._pwm = PWMGenerator() if pwm_generator is None else pwm_generator
53+
self._pin = pin
54+
self._angle = None
55+
self._min_angle_pulse = min_angle_pulse
56+
self._max_angle_pulse = max_angle_pulse
57+
self._angle_range = angle_range
58+
self._frequency = frequency
59+
60+
@property
61+
def angle(self) -> Union[int, None]:
62+
""":obj:`int` or :obj:`None`: Angle of the servo in degrees."""
63+
return self._angle
64+
65+
@angle.setter
66+
def angle(self, value: int):
67+
duty_cycle = self._get_duty_cycle(value)
68+
self._pwm.generate(self._pin, self._frequency, duty_cycle)
69+
self._angle = value
70+
71+
def _get_duty_cycle(self, angle):
72+
angle /= self._angle_range # Normalize
73+
angle *= self._max_angle_pulse - self._min_angle_pulse # Scale
74+
angle += self._min_angle_pulse # Offset
75+
return angle / (self._frequency ** -1 * MICROSECONDS)

‎PSL/sciencelab.py‎

Lines changed: 0 additions & 87 deletions
Original file line numberDiff line numberDiff line change
@@ -119,13 +119,6 @@ def __runInitSequence__(self, **kwargs):
119119

120120
self.DAC = MCP4728(self.H, 3.3, 0)
121121

122-
def get_resistance(self):
123-
V = self.get_average_voltage('RES')
124-
if V > 3.295: return np.Inf
125-
I = (3.3 - V) / 5.1e3
126-
res = V / I
127-
return res * self.resistanceScaling
128-
129122
def __print__(self, *args):
130123
if self.verbose:
131124
for a in args:
@@ -649,86 +642,6 @@ def __write_data_address__(self, address, value):
649642
self.H.__sendInt__(value)
650643
self.H.__get_ack__()
651644

652-
# -------------------------------------------------------------------------------------------------------------------#
653-
654-
# |==============================================MOTOR SIGNALLING====================================================|
655-
# |Set servo motor angles via SQ1-4. Control one stepper motor using SQ1-4 |
656-
# -------------------------------------------------------------------------------------------------------------------#
657-
658-
def __stepperMotor__(self, steps, delay, direction):
659-
self.H.__sendByte__(CP.NONSTANDARD_IO)
660-
self.H.__sendByte__(CP.STEPPER_MOTOR)
661-
self.H.__sendInt__((steps << 1) | direction)
662-
self.H.__sendInt__(delay)
663-
664-
time.sleep(steps * delay * 1e-3) # convert mS to S
665-
666-
def stepForward(self, steps, delay):
667-
"""
668-
Control stepper motors using SQR1-4
669-
670-
take a fixed number of steps in the forward direction with a certain delay( in milliseconds ) between each step.
671-
672-
"""
673-
self.__stepperMotor__(steps, delay, 1)
674-
675-
def stepBackward(self, steps, delay):
676-
"""
677-
Control stepper motors using SQR1-4
678-
679-
take a fixed number of steps in the backward direction with a certain delay( in milliseconds ) between each step.
680-
681-
"""
682-
self.__stepperMotor__(steps, delay, 0)
683-
684-
def servo(self, angle, chan='SQ1'):
685-
'''
686-
Output A PWM waveform on SQR1/SQR2 corresponding to the angle specified in the arguments.
687-
This is used to operate servo motors. Tested with 9G SG-90 Servo motor.
688-
689-
.. tabularcolumns:: |p{3cm}|p{11cm}|
690-
691-
============== ============================================================================================
692-
**Arguments**
693-
============== ============================================================================================
694-
angle 0-180. Angle corresponding to which the PWM waveform is generated.
695-
chan 'SQ1' or 'SQ2'. Whether to use SQ1 or SQ2 to output the PWM waveform used by the servo
696-
============== ============================================================================================
697-
'''
698-
self.pwm_generator.generate(chan, frequency=100, duty_cycles=7.5 + 19 * angle / 180)
699-
700-
def servo4(self, a1, a2, a3, a4):
701-
"""
702-
Operate Four servo motors independently using SQR1, SQR2, SQR3, SQR4.
703-
tested with SG-90 9G servos.
704-
For high current servos, please use a different power source, and a level convertor for the PWm output signals(if needed)
705-
706-
.. tabularcolumns:: |p{3cm}|p{11cm}|
707-
708-
============== ============================================================================================
709-
**Arguments**
710-
============== ============================================================================================
711-
a1 Angle to set on Servo which uses SQR1 as PWM input. [0-180]
712-
a2 Angle to set on Servo which uses SQR2 as PWM input. [0-180]
713-
a3 Angle to set on Servo which uses SQR3 as PWM input. [0-180]
714-
a4 Angle to set on Servo which uses SQR4 as PWM input. [0-180]
715-
============== ============================================================================================
716-
717-
"""
718-
params = (1 << 5) | 2 # continuous waveform. prescaler 2( 1:64)
719-
self.H.__sendByte__(CP.WAVEGEN)
720-
self.H.__sendByte__(CP.SQR4)
721-
self.H.__sendInt__(10000) # 10mS wavelength
722-
self.H.__sendInt__(750 + int(a1 * 1900 / 180))
723-
self.H.__sendInt__(0)
724-
self.H.__sendInt__(750 + int(a2 * 1900 / 180))
725-
self.H.__sendInt__(0)
726-
self.H.__sendInt__(750 + int(a3 * 1900 / 180))
727-
self.H.__sendInt__(0)
728-
self.H.__sendInt__(750 + int(a4 * 1900 / 180))
729-
self.H.__sendByte__(params)
730-
self.H.__get_ack__()
731-
732645
def enableUartPassthrough(self, baudrate, persist=False):
733646
'''
734647
All data received by the device is relayed to an external port(SCL[TX],SDA[RX]) after this function is called

‎tests/recordings/motor/test_get_angle.json‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1 @@
1+
[[[7], [7], [31, 78], [219, 5], [0, 0], [1, 0], [0, 0], [1, 0], [0, 0], [1, 0], [34], [8], [1], [224], [7], [8], [0], [0]], [[], [], [], [], [], [], [], [], [], [], [1], [], [], [1], [], [], [], [1]]]

‎tests/recordings/motor/test_set_angle.json‎

Lines changed: 1 addition & 0 deletions
Large diffs are not rendered by default.

‎tests/test_motor.py‎

Lines changed: 31 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,31 @@
1+
import pytest
2+
3+
from PSL.logic_analyzer import LogicAnalyzer
4+
from PSL.motor import Servo
5+
from PSL.packet_handler import Handler
6+
from PSL.waveform_generator import PWMGenerator
7+
8+
RELTOL = 0.01
9+
10+
11+
@pytest.fixture
12+
def servo(handler: Handler) -> Servo:
13+
handler._logging = True
14+
return Servo("SQ1", PWMGenerator(handler))
15+
16+
17+
@pytest.fixture
18+
def la(handler: Handler) -> LogicAnalyzer:
19+
handler._logging = True
20+
return LogicAnalyzer(handler)
21+
22+
23+
def test_set_angle(servo: Servo, la: LogicAnalyzer):
24+
servo.angle = 90
25+
wavelength, duty_cycle = la.measure_duty_cycle("LA1")
26+
assert wavelength * duty_cycle == pytest.approx(1500, rel=RELTOL)
27+
28+
29+
def test_get_angle(servo: Servo):
30+
servo.angle = 90
31+
assert servo.angle == 90

‎tox.ini‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -19,7 +19,7 @@ commands = coverage run --source PSL -m pytest --record
1919
[testenv:lint]
2020
deps = -rlint-requirements.txt
2121
setenv =
22-
INCLUDE_PSL_FILES = PSL/achan.py PSL/digital_channel.py PSL/logic_analyzer.py PSL/oscilloscope.py PSL/packet_handler.py PSL/waveform_generator.py PSL/multimeter.py
22+
INCLUDE_PSL_FILES = PSL/achan.py PSL/digital_channel.py PSL/logic_analyzer.py PSL/oscilloscope.py PSL/packet_handler.py PSL/waveform_generator.py PSL/multimeter.py PSL/motor.py
2323
commands =
2424
black --check {env:INCLUDE_PSL_FILES}
2525
flake8 --show-source {env:INCLUDE_PSL_FILES}

0 commit comments

Comments
 (0)