def __init__(self, port_name, zero_point=0, name="servo"):
self._pma_port = port_name
self.name = name
self.__controller = ServoController(self._pma_port)
self.__target_angle = 0.0
self.__target_speed = self.__DEFAULT_SPEED
self.__min_angle = self.__HARDWARE_MIN_ANGLE
self.__max_angle = self.__HARDWARE_MAX_ANGLE
self.__has_set_angle = False
self.__zero_point = zero_point
atexit.register(self.__cleanup)
Stateful.__init__(self)
Recreatable.__init__(
self,
config_dict={
"port_name": port_name,
"name": name,
"zero_point": lambda: self.zero_point,
},
)
def test_acceleration_mode_fails_on_invalid_type(self):
"""Can't set acceleration mode if provided mode has invalid type."""
controller = ServoController(port="S0")
for acceleration_mode in ("a", 0.1):
with self.assertRaises(TypeError):
controller.set_acceleration_mode(acceleration_mode)
def test_set_max_pulse_width_write(self):
"""Registers written when setting the maximum pulse width to MCU."""
# create instance
controller = ServoController(port="S1")
# setup mock
with patch.object(controller._mcu_device,
"write_word") as write_word_mock:
max_pulse_width = 500
# test
controller.set_max_pulse_width(max_pulse_width)
write_word_mock.assert_called_with(
ServoMotorSetup.REGISTER_MAX_PULSE_WIDTH,
max_pulse_width,
signed=False,
little_endian=True,
)
def test_set_pwm_frequency_read_write(self):
"""Registers read/written when setting/reading PWM frequency to/from
MCU."""
# create instance
controller = ServoController(port="S0")
pwm_frequency_value = 200
with patch.object(controller, "_mcu_device") as mcu_device_mock:
# setup r/w mocks
write_byte_mock = mcu_device_mock.write_byte = Mock()
read_unsigned_byte_mock = mcu_device_mock.read_unsigned_byte = Mock(
return_value=pwm_frequency_value)
# test
controller.set_pwm_frequency(pwm_frequency_value)
write_byte_mock.assert_called_with(
ServoMotorSetup.REGISTER_PWM_FREQUENCY, pwm_frequency_value)
self.assertEquals(controller.pwm_frequency(), pwm_frequency_value)
read_unsigned_byte_mock.assert_called_with(
ServoMotorSetup.REGISTER_PWM_FREQUENCY)
def test_acceleration_mode_read_write(self):
"""Registers read/written when setting/reading acceleration mode
to/from MCU."""
# create instance
controller = ServoController(port="S0")
with patch.object(controller, "_mcu_device") as mcu_device_mock:
for acceleration_mode in (0, 1):
# setup r/w mocks
write_byte_mock = mcu_device_mock.write_byte = Mock()
read_unsigned_byte_mock = mcu_device_mock.read_unsigned_byte = Mock(
return_value=acceleration_mode)
# test
controller.set_acceleration_mode(acceleration_mode)
write_byte_mock.assert_called_with(
ServoMotorS0.get(ServoRegisterTypes.ACC_MODE),
acceleration_mode)
self.assertEquals(controller.get_acceleration_mode(),
acceleration_mode)
read_unsigned_byte_mock.assert_called_with(
ServoMotorS0.get(ServoRegisterTypes.ACC_MODE))
class ServoMotor(Stateful, Recreatable):
"""Represents a pi-top servo motor component.
Note that pi-top servo motors use an open-loop control system. As such, the output of the device (e.g.
the angle and speed of the servo horn) cannot be measured directly. This means that you can set a target
angle or speed for the servo, but you cannot read the current angle or speed.
:type port_name: str
:param port_name:
The ID for the port to which this component is connected.
:type zero_point: int
:param zero_point:
A user-defined offset from 'true' zero.
"""
__HARDWARE_MIN_ANGLE = -ServoHardwareSpecs.ANGLE_RANGE / 2
__HARDWARE_MAX_ANGLE = ServoHardwareSpecs.ANGLE_RANGE / 2
__DEFAULT_SPEED = 50.0
def __init__(self, port_name, zero_point=0, name="servo"):
self._pma_port = port_name
self.name = name
self.__controller = ServoController(self._pma_port)
self.__target_angle = 0.0
self.__target_speed = self.__DEFAULT_SPEED
self.__min_angle = self.__HARDWARE_MIN_ANGLE
self.__max_angle = self.__HARDWARE_MAX_ANGLE
self.__has_set_angle = False
self.__zero_point = zero_point
atexit.register(self.__cleanup)
Stateful.__init__(self)
Recreatable.__init__(
self,
config_dict={
"port_name": port_name,
"name": name,
"zero_point": lambda: self.zero_point,
},
)
@property
def own_state(self):
return {
"angle": self.current_angle,
"speed": self.current_speed,
}
def __cleanup(self):
if self.__has_set_angle and self.current_speed != 0.0:
self.stop()
def stop(self):
"""Stop servo at its current position.
:return: None
"""
self.target_angle = self.current_angle
@property
def zero_point(self):
"""Represents the servo motor angle that the library treats as 'zero'.
This value can be anywhere in the range of -90 to +90.
For example, if the zero_point were set to be -30, then the valid range
of values for setting the angle would be -60 to +120.
.. warning::
Setting a zero point out of the range of -90 to 90 will cause the method
to raise an exception.
"""
return self.__zero_point
@zero_point.setter
def zero_point(self, zero_position):
if not (self.__HARDWARE_MIN_ANGLE <= zero_position <=
self.__HARDWARE_MAX_ANGLE):
raise ValueError(
f"Value must be from {self.__HARDWARE_MIN_ANGLE} to {self.__HARDWARE_MAX_ANGLE} degrees "
f"(inclusive)")
self.__zero_point = zero_position
self.__min_angle = self.__HARDWARE_MIN_ANGLE - self.__zero_point
self.__max_angle = self.__HARDWARE_MAX_ANGLE - self.__zero_point
@property
def angle_range(self):
"""Returns a tuple with minimum and maximum possible angles where the
servo horn can be moved to.
If :class:`zero_point` is set to 0 (default), the angle range
will be (-90, 90).
"""
return self.__min_angle, self.__max_angle
@property
def setting(self):
"""Returns the current state of the servo motor, giving current angle
and current speed.
:return: :class:'ServoMotorSetting` object that has angle and speed attributes.
"""
if not self.__has_set_angle:
return None, None
angle, speed = self.__controller.get_current_angle_and_speed()
current_state = ServoMotorSetting()
current_state.angle = angle - self.zero_point
current_state.speed = speed
return current_state
@setting.setter
def setting(self, target_state: ServoMotorSetting):
"""Sets the target state of the servo horn, relative to the zero
position.
.. warning::
Using an :data:`target_state.angle` out of the valid angle range will cause the method to raise an
exception. To determine the valid angle range, use :meth:`ServoMotor.get_angle_range`.
.. warning::
Using a :data:`target_state.speed` out of the valid speed range will cause the method to raise an exception.
:type target_state: :class:`ServoMotorSetting`
:param target_state:
Set the target servo state using the :class:`ServoMotorSetting` class, both :meth:`ServoMotorSetting.speed` and
:meth:`ServoMotorSetting.angle` must be passed. Example usage:
.. code-block:: python
from pitop import ServoMotor, ServoMotorSetting
servo = ServoMotor()
target_setting = ServoMotorSetting()
target_setting.angle = 45
target_setting.speed = 20
servo.setting = target_setting
"""
self.__controller.set_target_angle(
target_state.angle + self.__zero_point, target_state.speed)
self.__has_set_angle = True
@property
def current_angle(self):
"""Returns the current angle that the servo motor is at.
.. note::
If you need synchronized angle and speed values, use :meth:`ServoMotor.state` instead, this will return both
current angle and current speed at the same time.
:return: float value of the current angle of the servo motor in degrees.
"""
if not self.__has_set_angle:
raise RuntimeError(
"Current angle is unknown. "
"Please set a servo angle first to initialise the servo to an angle."
)
angle, _ = self.__controller.get_current_angle_and_speed()
return angle - self.zero_point
@property
def current_speed(self):
"""Returns the current speed the servo motor is at.
.. note::
If you need synchronized angle and speed values, use :meth:`ServoMotor.state` instead, this will return both
current angle and current speed at the same time.
:return: float value of the current speed of the servo motor in deg/s.
"""
_, speed = self.__controller.get_current_angle_and_speed()
return speed
@property
def smooth_acceleration(self):
"""Gets whether or not the servo is configured to use a linear
acceleration profile to ramp speed at start and end of cycle.
:return: boolean value of the acceleration mode
"""
return self.__controller.get_acceleration_mode() == 1
@smooth_acceleration.setter
def smooth_acceleration(self, enabled: bool):
"""Sets whether or not the servo is configured to use a linear
acceleration profile to ramp speed at start and end of cycle.
:type enabled: bool
:param enabled: acceleration mode state
"""
self.__controller.set_acceleration_mode(int(enabled))
@property
def target_angle(self):
"""Returns the last target angle that has been set.
:return: float value of the target angle of the servo motor in deg.
"""
return self.__target_angle
@target_angle.setter
def target_angle(self, angle):
"""Set the target angle you want the servo motor to go to.
:type angle: float
:param angle: target servo motor angle.
"""
if not (self.__min_angle <= angle <= self.__max_angle):
raise ValueError(
f"Angle value must be from {self.__min_angle} to {self.__max_angle} degrees (inclusive)"
)
self.__target_angle = angle
self.__controller.set_target_angle(
self.__target_angle + self.__zero_point, self.__target_speed)
self.__has_set_angle = True
@property
def target_speed(self):
"""Returns the last target speed that has been set.
:return: float value of the target speed of the servo motor in deg/s.
"""
return self.__target_speed
@target_speed.setter
def target_speed(self, speed):
"""Sets the servo motor speed. The speed value must be a number from.
-100.0 to 100.0 deg/s.
.. warning::
Using a :data:`speed` out of the valid speed range will cause the method to raise an exception.
:type speed: int or float
:param speed:
The target speed at which to move the servo horn, from -100 to 100 deg/s.
"""
if not (-ServoHardwareSpecs.SPEED_RANGE <= speed <=
ServoHardwareSpecs.SPEED_RANGE):
raise ValueError(
f"Speed value must be from {ServoHardwareSpecs.SPEED_RANGE} to {ServoHardwareSpecs.SPEED_RANGE} deg/s (inclusive)"
)
self.__target_speed = speed
def sweep(self, speed=None):
"""Moves the servo horn from the current position to one of the servo
motor limits (maximum/minimum possible angle), moving at the specified
speed. The speed value must be a number from -100.0 to 100.0 deg/s.
The sweep direction is given by the speed.
Setting a :data:`speed` value higher than zero will move the horn to the maximum angle (90 degrees by default),
while a value less than zero will move it to the minimum angle (-90 degress by default).
.. warning::
Using a :data:`speed` out of the valid speed range will cause the method to raise an exception.
:type speed: int or float
:param speed:
The target speed at which to move the servo horn, from -100 to 100 deg/s.
"""
speed = self.target_speed if speed is None else speed
if not (-ServoHardwareSpecs.SPEED_RANGE <= speed <=
ServoHardwareSpecs.SPEED_RANGE):
raise ValueError(
f"Speed value must be from {ServoHardwareSpecs.SPEED_RANGE} to {ServoHardwareSpecs.SPEED_RANGE} deg/s (inclusive)"
)
angle = self.__min_angle if speed < 0 else self.__max_angle
self.__controller.set_target_angle(angle + self.__zero_point, speed)
def test_constructor_fails_on_incorrect_port(self):
"""Constructor fails if providing an invalid port."""
with self.assertRaises(Exception):
ServoController(port="invalid_port")
def test_constructor_success(self):
controller = ServoController(port="S0")
self.assertEquals(controller.registers, ServoMotorS0)