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