def set_target_angle(self,
angle: Union[int, float],
speed: Union[int, float] = 50.0):
mapped_duty_cycle = interp(
angle,
[
-ServoHardwareSpecs.ANGLE_RANGE / 2,
ServoHardwareSpecs.ANGLE_RANGE / 2
],
[self.__lower_duty_cycle, self.__upper_duty_cycle],
)
mapped_duty_cycle = int(round(mapped_duty_cycle))
mapped_speed = int(round(speed * 10))
list_to_send = split_into_bytes(mapped_duty_cycle,
2,
signed=True,
little_endian=True)
list_to_send += split_into_bytes(mapped_speed,
2,
signed=True,
little_endian=True)
self._mcu_device.write_n_bytes(
self.registers[ServoRegisterTypes.ANGLE_AND_SPEED], list_to_send)
def set_rpm_with_rotations(self, rpm: int, rotations_to_complete: int) -> None:
self.set_control_mode(MotorControlModes.MODE_2)
list_to_send = split_into_bytes(
rotations_to_complete, 2, signed=True, little_endian=True
) + split_into_bytes(rpm, 2, signed=True, little_endian=True)
self._mcu_device.write_n_bytes(
self.registers[MotorRegisterTypes.MODE_2_RPM_WITH_ROTATIONS], list_to_send
)
def test_split_into_bytes_signed(self, data: int, no_of_bytes: int,
expected_list):
test_bytes_list = bitwise.split_into_bytes(data,
no_of_bytes,
signed=True)
self.assertEqual(expected_list, test_bytes_list)
def read_all(self):
if self.__connect() is False:
print("Could not connect to device")
return self.__error_array
results_reading = self.__device.read_n_unsigned_bytes(
self.__register_address, number_of_bytes=self.__channel_count)
results = split_into_bytes(results_reading,
self.__channel_count,
little_endian=False)
data_read_len = len(results)
self.__disconnect()
if data_read_len != self.__channel_count:
print(
"Bad read from device. "
f"Expected {str(self.__channel_count)} bytes, received {str(data_read_len)} bytes."
)
return self.__error_array
for i in range(self.__channel_count):
results[i] *= self.__adc_ratio
results[i] = int(results[i])
return results
def get_current_angle_and_speed(self):
duty_cycle_and_speed = self._mcu_device.read_n_signed_bytes(
self.registers[ServoRegisterTypes.ANGLE_AND_SPEED],
number_of_bytes=4,
little_endian=True,
)
angle_speed_bytes = split_into_bytes(duty_cycle_and_speed,
no_of_bytes=4,
little_endian=True,
signed=True)
duty_cycle = int.from_bytes(angle_speed_bytes[0:2],
byteorder="little",
signed=True)
angle = int(
round(
interp(
duty_cycle,
[self.__lower_duty_cycle, self.__upper_duty_cycle],
[
-ServoHardwareSpecs.ANGLE_RANGE / 2,
ServoHardwareSpecs.ANGLE_RANGE / 2,
],
)))
speed = (int.from_bytes(
angle_speed_bytes[2:4], byteorder="little", signed=True) / 10.0)
return angle, speed
def write_word(self, register_address: int, word_value: int,
little_endian: bool, signed: bool):
bytes_to_write = split_into_bytes(word_value,
2,
little_endian=little_endian,
signed=signed)
if bytes_to_write is None:
logger.error(
f"Error splitting word into bytes list. Value: {word_value}")
return
self.write_n_bytes(register_address, bytes_to_write)
def test_control_mode_2_read_write(self):
"""Registers read/written when using control mode 2 methods."""
rpm_value = 500
rotations_value = 10
# from rpm and rotations values, calculate byte values to use in mocks
rpm_and_rotations_in_bytes = split_into_bytes(
rotations_value, 2, signed=True,
little_endian=True) + split_into_bytes(
rpm_value, 2, signed=True, little_endian=True)
rpm_and_rotations_read = join_bytes(rpm_and_rotations_in_bytes)
for motor_port_registers in MotorControlRegisters:
motor_port_name = motor_port_registers.name
motor_registers = motor_port_registers.value
mode_1_register = motor_registers[
MotorRegisterTypes.MODE_2_RPM_WITH_ROTATIONS]
# create instance
controller = EncoderMotorController(port=motor_port_name)
controller.set_control_mode = Mock()
controller.control_mode = Mock()
controller.control_mode.return_value = MotorControlModes.MODE_2
# setup r/w mocks
write_n_bytes_mock = controller._mcu_device.write_n_bytes = Mock()
read_n_unsigned_bytes_mock = (
controller._mcu_device.read_n_unsigned_bytes) = Mock()
read_n_unsigned_bytes_mock.return_value = rpm_and_rotations_read
# test
controller.set_rpm_with_rotations(rpm_value, rotations_value)
write_n_bytes_mock.assert_called_with(mode_1_register,
rpm_and_rotations_in_bytes)
self.assertEquals(controller.rpm_with_rotations(),
(rpm_value, rotations_value))
read_n_unsigned_bytes_mock.assert_called_with(mode_1_register,
4,
little_endian=True)
def rpm_with_rotations(self) -> Tuple[int, int]:
if self.control_mode() != MotorControlModes.MODE_2:
return None
value = self._mcu_device.read_n_unsigned_bytes(
self.registers[MotorRegisterTypes.MODE_2_RPM_WITH_ROTATIONS],
4,
little_endian=True,
)
value_in_bytes = split_into_bytes(value, 4, little_endian=True)
speed = join_bytes(value_in_bytes[0:2])
rotations = join_bytes(value_in_bytes[2:4])
return speed, rotations
def set_odometer(self, rotations: int) -> None:
list_to_send = split_into_bytes(rotations, 4, signed=True, little_endian=True)
self._mcu_device.write_n_bytes(
self.registers[MotorRegisterTypes.ODOMETER], list_to_send
)