def test_process_stop_command_distance_break(self):
creator = MotorCommandProcessor()
command = StopCommand('ev3-ports:outA', -500, 'break')
spf, frames, run_time = creator.process_stop_command_distance(command)
self.assertEqual(spf, 0)
self.assertEqual(frames, 1)
self.assertEqual(run_time, 0)
def test_process_stop_command_degrees_hold(self):
creator = MotorCommandProcessor()
command = StopCommand('ev3-ports:outA', 100, 'hold')
spf, frames, run_time = creator.process_stop_command_degrees(command)
self.assertEqual(spf, 0)
self.assertEqual(frames, 0)
self.assertEqual(run_time, 0)
def test_to_mm_per_frame(self):
creator = MotorCommandProcessor()
mm_check = (wheel_circumference / 360 * 730) / 100
ppf = creator._to_mm_per_frame(100, 730)
self.assertAlmostEqual(ppf, mm_check, 3)
mm_check = (wheel_circumference / 360 * -730) / 100
ppf = creator._to_mm_per_frame(100, -730)
self.assertAlmostEqual(ppf, mm_check, 3)
def test_process_stop_command_degrees_coast(self):
creator = MotorCommandProcessor()
command = StopCommand('ev3-ports:outA', 500, 'coast')
spf, frames, run_time = creator.process_stop_command_degrees(command)
self.assertAlmostEqual(spf, 16.667, 3)
self.assertEqual(frames, 11)
self.assertAlmostEqual(run_time, 0.367, 3)
def test_to_mm(self):
creator = MotorCommandProcessor()
mm_check = wheel_circumference / 360 * 720
mm = creator._to_mm(720)
self.assertAlmostEqual(mm, mm_check, 3)
mm_check = wheel_circumference / 360 * -720
mm = creator._to_mm(-720)
self.assertAlmostEqual(mm, mm_check, 3)
def test_coast_frames_required(self):
coasting_sub = get_simulation_settings(
)['motor_settings']['distance_coasting_subtraction']
creator = MotorCommandProcessor()
frames = creator._coast_frames_required(20, coasting_sub)
self.assertEqual(frames, int(round((20 / coasting_sub))), 5)
frames = creator._coast_frames_required(-20, coasting_sub)
self.assertEqual(frames, int(round((20 / coasting_sub))), 5)
def test_process_drive_command_degrees_coast(self):
creator = MotorCommandProcessor()
command = RotateCommand('ev3-ports:outA', 500, 1000, 'coast')
spf, frames, coast_frames, run_time = creator.process_drive_command_degrees(
command)
self.assertAlmostEqual(spf, 16.667, 3)
self.assertEqual(frames, 60)
self.assertEqual(coast_frames, 11)
self.assertAlmostEqual(run_time, 2.367, 3)
def test_process_drive_command_degrees_hold(self):
creator = MotorCommandProcessor()
command = RotateCommand('ev3-ports:outA', 100, 1000, 'hold')
# 1000 degrees, 100 per second
spf, frames, coast_frames, run_time = creator.process_drive_command_degrees(
command)
self.assertAlmostEqual(spf, 3.333, 3)
self.assertEqual(frames, 300)
self.assertEqual(coast_frames, 0)
self.assertEqual(run_time, 10)
def test_process_drive_command_distance_hold(self):
creator = MotorCommandProcessor()
command = RotateCommand('ev3-ports:outA', 100, 1000, 'hold')
spf, frames, coast_frames, run_time = creator.process_drive_command_distance(
command)
frames_check = 10 * 30 # (1000/100) * 30 # distance * fps
distance_in_mm = 1000 / 360 * wheel_circumference
dpf = distance_in_mm / frames_check
self.assertAlmostEqual(spf, dpf, 3)
self.assertEqual(frames, 300)
self.assertEqual(coast_frames, 0)
self.assertEqual(run_time, 10)
def __init__(self, brick_id: int, robot_sim: robot_simulator):
cfg = get_simulation_settings()
self.brick_id = brick_id
self.distance_coasting_sub = float(
cfg['motor_settings']['distance_coasting_subtraction'])
self.degree_coasting_sub = float(
cfg['motor_settings']['degree_coasting_subtraction'])
self.frames_per_second = int(cfg['exec_settings']['frames_per_second'])
self.robot_sim = robot_sim
self.command_processor = MotorCommandProcessor()
self.led_cache = {k: None for k in LEDS.values()}
def test_process_stop_command_distance_coast(self):
creator = MotorCommandProcessor()
command = StopCommand('ev3-ports:outA', 500, 'coast')
distance_coasting_sub = 0.7
spf, frames, run_time = creator.process_stop_command_distance(command)
mm_per_second = 500 / 360 * wheel_circumference
mm_per_frame = mm_per_second / 30 # divide by fps
frames_check = round(mm_per_frame / distance_coasting_sub)
self.assertAlmostEqual(spf, mm_per_frame, 3)
self.assertEqual(frames, frames_check)
self.assertAlmostEqual(run_time, 0.4, 3)
def test_frames_required(self):
creator = MotorCommandProcessor()
frames = creator._frames_required(20, 100)
self.assertEqual(frames, 150)
frames = creator._frames_required(20, -100)
self.assertEqual(frames, 150)
frames = creator._frames_required(33, 1000)
self.assertEqual(frames, 909)
frames = creator._frames_required(-33, 1000)
self.assertEqual(frames, 909)
frames = creator._frames_required(-66, -700)
self.assertEqual(frames, 318)
class MessageProcessor:
"""
Class used for processing the messages (requests/commands) received by the socket of the simulator and relaying
them to the RobotState.
"""
def __init__(self, brick_id: int, robot_sim: robot_simulator):
cfg = get_simulation_settings()
self.brick_id = brick_id
self.distance_coasting_sub = float(
cfg['motor_settings']['distance_coasting_subtraction'])
self.degree_coasting_sub = float(
cfg['motor_settings']['degree_coasting_subtraction'])
self.frames_per_second = int(cfg['exec_settings']['frames_per_second'])
self.robot_sim = robot_sim
self.command_processor = MotorCommandProcessor()
self.led_cache = {k: None for k in LEDS.values()}
def process_rotate_command(self, command: rotate_command) -> float:
"""
Process the given RotateCommand by creating the appropriate motor jobs in the RobotState.
The type of jobs created depends on the motor called.
The command for the arm motor is processed for degrees, while the other motors are processed for distance.
:param command: to process.
:return: a floating point value representing the time in seconds the given command will take to execute.
"""
full_address = self._to_full_address(command.address)
motor = self.robot_sim.robot.get_actuator(full_address)
spf, frames, coast_frames, run_time = self._process_rotate_command_values(
command, motor)
self.robot_sim.clear_actuator_jobs(full_address)
for _ in range(frames):
self.robot_sim.put_actuator_job(full_address, spf)
self._process_coast(coast_frames, spf, motor)
return run_time
def _process_rotate_command_values(
self, command: rotate_command,
motor: any) -> Tuple[float, int, int, float]:
"""
Process the given command into the correct movement values.
:param command: to process.
:param motor: the motor.
:return: a Tuple with the processed values
"""
if motor.ev3type == 'arm':
dpf, frames, coast_frames, run_time = self.command_processor.process_drive_command_degrees(
command)
return -dpf, frames, coast_frames, run_time
return self.command_processor.process_drive_command_distance(command)
def process_stop_command(self, command: stop_command) -> float:
"""
Process the given stop command by clearing the current motor job queue
and creating motor coast jobs in the RobotState. The type of jobs created
depends on the motor called. The command for the center motor is processed for degrees, while the other motors
are processed for distance.
:param command: to process.
:return: a floating point value representing the time in seconds the given command will take to execute.
"""
full_address = self._to_full_address(command.address)
motor = self.robot_sim.robot.get_actuator(full_address)
spf, frames, run_time = self._process_stop_command_values(
command, motor)
self.robot_sim.clear_actuator_jobs(full_address)
self._process_coast(frames, spf, motor)
return run_time
def _process_stop_command_values(self, command: stop_command,
motor: any) -> Tuple[float, int, float]:
"""
Process the given command into the correct movement values.
:param command: to process.
:param motor: the motor.
:return: a Tuple with the processed values
"""
if motor.ev3type == 'arm':
dpf, frames, run_time = self.command_processor.process_stop_command_degrees(
command)
return -dpf, frames, run_time
return self.command_processor.process_stop_command_distance(command)
def _process_coast(self, frames, ppf, motor):
"""
Process coasting by creating move jobs decreasing in speed in the RobotState.
:param frames: to coast before coming to a halt.
:param ppf: speed of motor when the coasting starts.
:param motor: the motor in the RobotState.
"""
coasting_sub = self.degree_coasting_sub if motor.ev3type == 'arm' else self.distance_coasting_sub
og_ppf = ppf
for _ in range(frames):
if og_ppf > 0:
ppf = max(ppf - coasting_sub, 0)
else:
ppf = min(ppf + coasting_sub, 0)
self.robot_sim.put_actuator_job(
self._to_full_address(motor.address), ppf)
def process_led_command(self, command: led_command):
"""
Process the given sound command by creating a sound job with a message which can be put on the simulator screen.
:param command: to process.
"""
self.led_cache[command.address] = command.brightness
led_id = command.get_led_id()
color_tuple = (self.led_cache[led_id + ':red:brick-status'],
self.led_cache[led_id + ':green:brick-status'])
color = self._red_green_to_color(color_tuple)
if color is not None:
self.robot_sim.set_led_color(self.brick_id, led_id, color)
@staticmethod
def _red_green_to_color(color_tuple):
if len(color_tuple) != 2:
return None
if color_tuple[0] is None:
color_tuple = (0, color_tuple[1])
if color_tuple[1] is None:
color_tuple = (color_tuple[0], 0)
color = LED_COLORS.get(color_tuple)
diff = 2
if color is None:
for key, value in LED_COLORS.items():
new_diff = abs(color_tuple[0] - key[0]) + abs(color_tuple[1] -
key[1])
if new_diff < diff:
color = value
diff = new_diff
return color
def process_sound_command(self, command: sound_command):
"""
Process the given sound command by creating a sound job with a message which can be put on the simulator screen.
:param command: to process.
"""
frames = int(round(self.frames_per_second * command.duration))
msg_len = len(command.message)
message = '\n'.join(command.message[i:i + 10]
for i in range(0, msg_len, 10))
for i in range(frames):
self.robot_sim.put_actuator_job(self._to_full_address('speaker'),
message)
def process_data_request(self, request: data_request) -> Any:
"""
Process the given data request by retrieving the requested value from the RobotState and returning this.
:param request: to process.
:return: a dictionary containing the requested value.
"""
full_address = self._to_full_address(request.address)
return self.robot_sim.get_value(full_address)
def process_config_request(self, request: ConfigRequest) -> Any:
"""
Process the given data request by retrieving the port of the device from the RobotState and returning this.
:param request: to process.
:return: a dictionary containing the requested value.
"""
return self.robot_sim.determine_port(self.brick_id, request.kwargs,
request.class_name)
def _to_full_address(self, address: str):
return self.brick_id, address