async def test_set_position_pids(socket_adapter_mock, configuration_test):
"""
Test that the motor gateway encodes a pid configuration message, with float values.
Assumes that speed PIDs are initially 0, if never set before.
"""
motor_gateway = MotorGateway(socket_adapter_mock, configuration_test)
await motor_gateway.set_pid_position(1 * pi, 2 * pi, 3 * pi, 4 * pi,
5 * pi, 6 * pi)
packet, _ = socket_adapter_mock.send.call_args
message = BusMessage()
message.ParseFromString(*packet)
left_pid = PIDCoefficients(
kp=1 * pi,
ki=2 * pi,
kd=3 * pi,
)
right_pid = PIDCoefficients(
kp=4 * pi,
ki=5 * pi,
kd=6 * pi,
)
# If we want to set the position PIDs, speed PIDs should be set to 0
assert message == BusMessage(pidConfig=PIDConfigMsg(
pid_speed_left=PIDCoefficients(kp=0, ki=0, kd=0),
pid_speed_right=PIDCoefficients(kp=0, ki=0, kd=0),
pid_position_left=left_pid,
pid_position_right=right_pid))
async def decode_message(self, msg: bytes, source: str) -> None:
""" Convert bytes to BusMessage. """
bus_message = BusMessage()
bus_message.ParseFromString(msg)
printable_msg = json_format.MessageToJson(
bus_message, including_default_value_fields=True)
LOGGER.get().debug('msg_can', msg=printable_msg, source=source)
await self.dispatch_message(bus_message, source)
async def test_set_tolerances(socket_adapter_mock, configuration_test):
"""
Test that the motor gateway encodes a wheel tolerance message.
"""
motor_gateway = MotorGateway(socket_adapter_mock, configuration_test)
await motor_gateway.set_tolerances(10, 20)
packet, _ = socket_adapter_mock.send.call_args
message = BusMessage()
message.ParseFromString(*packet)
assert message == BusMessage(
wheelTolerances=WheelTolerancesMsg(ticks_left=10, ticks_right=20))
async def test_set_position(socket_adapter_mock, configuration_test):
"""
Test that the motor gateway encodes a wheel target positon message.
"""
motor_gateway = MotorGateway(socket_adapter_mock, configuration_test)
await motor_gateway.set_target_positions(10, 20)
packet, _ = socket_adapter_mock.send.call_args
message = BusMessage()
message.ParseFromString(*packet)
assert message == BusMessage(
wheelPositionTarget=WheelPositionTargetMsg(tick_left=10,
tick_right=20))
async def test_set_speed(socket_adapter_mock, configuration_test):
"""
Test that the motor gateway encodes a wheel target speed message.
"""
motor_gateway = MotorGateway(motor_board_adapter=socket_adapter_mock,
configuration=configuration_test)
await motor_gateway.set_target_speeds(10, 20)
packet, _ = socket_adapter_mock.send.call_args
message = BusMessage()
message.ParseFromString(*packet)
assert message == BusMessage(moveWheelAtSpeed=MoveWheelAtSpeedMsg(
left_tick_per_sec=10, right_tick_per_sec=20))
async def callback(bytes, name):
global t_last
t = time.time()
bus_message = BusMessage()
try:
bus_message.ParseFromString(bytes)
printable_data = json_format.MessageToDict(
bus_message, including_default_value_fields=True)
json_data = json.dumps(printable_data)
sys.stdout.write(f'{(t - t_last) * 1000:10.3f} ms:"{name}" ' +
json_data + '\n')
sys.stdout.flush()
except DecodeError:
print("Protobuf couldn't decode this:", bytes)
t_last = t
async def handle_movement_order(self, data: bytes, _: str) -> None:
"""
Handle move wheels packets.
"""
bus_message = BusMessage()
bus_message.ParseFromString(data)
# pylint: disable=no-member
type_msg = bus_message.WhichOneof("message_content")
if type_msg == "moveWheelAtSpeed":
target_left = bus_message.moveWheelAtSpeed.left_tick_per_sec
target_right = bus_message.moveWheelAtSpeed.right_tick_per_sec
self.simulation_state.queue_speed_left.append(target_left)
self.simulation_state.queue_speed_left.popleft()
self.simulation_state.queue_speed_right.append(target_right)
self.simulation_state.queue_speed_right.popleft()
LOGGER.get().debug('simulation_router_received_wheel_speed_target')
elif type_msg == "pidConfig":
LOGGER.get().debug('simulation_router_received_pid_config')
else:
LOGGER.get().debug('simulation_router_received_unhandled_order')
async def translate_message(self, msg: bytes) -> None:
""" Convert bytes to BusMessage. """
bus_message = BusMessage()
bus_message.ParseFromString(msg)
LOGGER.get().debug('msg_can', msg=bus_message)
await self.dispatch_message(bus_message)
