def test_run_to_rel_pos(self): self.clientSocketMock.send_command.return_value = 1 motor = Motor(OUTPUT_A) motor.on_for_degrees(30, 30, False) self.assertEqual(len(self.clientSocketMock.mock_calls), 1) fn_name, args, kwargs = self.clientSocketMock.mock_calls[0] self.assertEqual(fn_name, 'send_command') self.assertDictEqual(args[0].serialize(), {'type': 'RotateCommand', 'address': 'ev3-ports:outA', 'stop_action': 'coast', 'speed': 315, 'distance': 30})
class TouchyArm(object): def __init__(self, output, address): self.motor = Motor(output) self.sensor = TouchSensor(address) self.should_wink = False def put_down(self): self.motor.on_to_position(SpeedPercent(20), 90, block=True) def put_up(self): self.motor.on_to_position(SpeedPercent(20), 0, block=True) def reset(self): self.motor.on_to_position(SpeedPercent(20), 0) async def winking(self): self.should_wink = True while self.should_wink: self.motor.on_for_degrees(SpeedPercent(100), -20, block=True) self.motor.on_for_degrees(SpeedPercent(50), 20, block=True) await asyncio.sleep(random.uniform(0.5, 2)) self.motor.on_for_degrees(SpeedPercent(100), -20, block=True) self.motor.on_for_degrees(SpeedPercent(50), 20, block=True) await asyncio.sleep(random.uniform(2, 3)) def stop_winking(self): self.should_wink = False async def wait_until_pressed(self): while True: if self.sensor.is_pressed: break await asyncio.sleep(0.1)
def MoveLinear(self, x2, y2, z2): self._logger.info("x2=%f, y2=%f, z2=%f", x2, y2, z2) mX = Motor(OUTPUT_A) mY = Motor(OUTPUT_B) mZ = Motor(OUTPUT_C) (speedX, speedY) = self.Speed(self.x1, self.y1, x2, y2) dx = x2 - self.x1 dy = y2 - self.y1 dz = z2 - self.z1 mZ.on_for_degrees(self.speedMax, self.oneMmZ*dz, block=True) mX.on_for_degrees(speedX, self.oneMmX*dx, block=False) mY.on_for_degrees(speedY, self.oneMmY*dy, block=False) mX.wait_until_not_moving() mY.wait_until_not_moving() mZ.wait_until_not_moving() self.x1 = x2 self.y1 = y2 self.z1 = z2
class IO(): """ Provides functions for high-level IO operations (move left, move forward, is left side free to move etc.). """ def __init__(self): # Large motors self.lm_left_port = "outB" self.lm_right_port = "outA" self.lm_left = LargeMotor(self.lm_left_port) self.lm_right = LargeMotor(self.lm_right_port) # distance at which sensor motor start moving self.move_sensor_check_degrees = 400 self.move_degrees = 570 # one tile distance self.move_speed = 35 self.after_crash_degrees = 200 self.steering_turn_speed = 30 # turning left or right self.steering_turn_degrees = 450 self.steering_turn_fwd_degrees = 150 # distance to move after turning # distance at which sensors start spinning self.steering_sensor_check_degrees = 50 self.btn = Button() # small motor self.sm_port = "outC" self.sm = Motor(self.sm_port) self.sm_turn_speed = 30 self.sm_center_turn_angle = 90 self.sm_side_turn_angle = 110 self.sm_is_left = False # color sensor self.color_sensor_port = "in1" self.color_sensor = ColorSensor(self.color_sensor_port) self.color_sensor.mode = ColorSensor.MODE_COL_REFLECT self.color_sensor_values = [] # regulations self.regulation_desired_value = 4 self.regulation_max_diff = 3 self.regulation_p = 1.5 self.regulation_tick = 0.03 # ultrasonic sensor self.ultrasonic_sensor_port = "in4" self.ultrasonic_sensor = UltrasonicSensor(self.ultrasonic_sensor_port) self.ultrasonic_sensor.mode = 'US-DIST-CM' self.ultrasonic_tile_length = 30 self.ultrasonic_max_value = 255 self.ultrasonic_sensor_values = [] # touch sensors self.touch_right = TouchSensor("in2") self.touch_left = TouchSensor("in3") def go_left(self): ok = self.__turn_left() if (ok): ok = self.__move_reg(self.steering_turn_fwd_degrees, self.steering_sensor_check_degrees) return ok def go_right(self): ok = self.__turn_right() if (ok): ok = self.__move_reg(self.steering_turn_fwd_degrees, self.steering_sensor_check_degrees) return ok def go_forward(self): return self.__move_reg(self.move_degrees, self.move_sensor_check_degrees) def go_back(self): self.__turn(stop_motor=self.lm_left, turn_motor=self.lm_right, degrees=self.steering_turn_degrees, speed=self.steering_turn_speed) return self.__turn(stop_motor=self.lm_right, turn_motor=self.lm_left, degrees=self.steering_turn_degrees, speed=-self.steering_turn_speed) def after_crash(self): debug_print("crash") self.__move(self.after_crash_degrees, self.move_speed) self.read_sensors() def __turn(self, stop_motor: Motor, turn_motor: Motor, degrees: int, speed: int): stop_motor.stop() start = turn_motor.degrees turn_motor.on(speed) while (abs(turn_motor.degrees - start) < degrees): if (not self.__check_button()): self.lm_left.stop() self.lm_right.stop() return False return True def __turn_left(self): return self.__turn(stop_motor=self.lm_left, turn_motor=self.lm_right, degrees=self.steering_turn_degrees, speed=-self.steering_turn_speed) def __turn_right(self): return self.__turn(stop_motor=self.lm_right, turn_motor=self.lm_left, degrees=self.steering_turn_degrees, speed=-self.steering_turn_speed) def __move(self, degrees, speed) -> None: self.lm_left.on_for_degrees(speed, degrees, block=False) self.lm_right.on_for_degrees(speed, degrees, block=True) def __reg(self): val = self.color_sensor.reflected_light_intensity diff = (self.regulation_desired_value - val) if diff >= 0 and val > 0: diff = min(diff, self.regulation_max_diff) elif val == 0: diff = 0 else: diff = -min(abs(diff), self.regulation_max_diff) diff *= self.regulation_p if self.sm_is_left: return (-self.move_speed + diff, -self.move_speed - diff) return (-self.move_speed - diff, -self.move_speed + diff) def __check_button(self): timeout = time.time() + self.regulation_tick while (time.time() <= timeout): # check for touch sensor if (self.touch_left.is_pressed or self.touch_right.is_pressed): return False if (self.btn.left or self.btn.right): debug_print("pressed") return None time.sleep(0.01) return True def __move_reg(self, degrees, sensor_degrees): t = Thread(target=self.read_sensors) start_l, start_r = (self.lm_left.degrees, self.lm_right.degrees) distance_l, distance_r = 0, 0 while (distance_l < degrees or distance_r < degrees): speed_l, speed_r = self.__reg() self.lm_left.on(speed_l, brake=True) self.lm_right.on(speed_r, brake=True) ok = self.__check_button() if (ok is False): self.lm_left.stop() self.lm_right.stop() return False elif (ok is None): debug_print("none") self.lm_left.stop() self.lm_right.stop() return None if ((distance_l >= sensor_degrees or distance_r >= sensor_degrees) and not t.isAlive()): t.start() distance_l = abs(start_l - self.lm_left.degrees) distance_r = abs(start_r - self.lm_right.degrees) t.join() return True def read_sensors(self): self.color_sensor_values = [] # List[float] self.ultrasonic_sensor_values = [] speed = self.sm_turn_speed if (self.sm_is_left): speed = -self.sm_turn_speed # side 1 self.color_sensor_values.append( self.color_sensor.reflected_light_intensity) self.ultrasonic_sensor_values.append( self.ultrasonic_sensor.distance_centimeters) # center self.sm.on_for_degrees(speed, self.sm_center_turn_angle) self.color_sensor_values.append( self.color_sensor.reflected_light_intensity) self.ultrasonic_sensor_values.append( self.ultrasonic_sensor.distance_centimeters) # side 2 self.sm.on_for_degrees(speed, self.sm_side_turn_angle) self.color_sensor_values.append( self.color_sensor.reflected_light_intensity) self.ultrasonic_sensor_values.append( self.ultrasonic_sensor.distance_centimeters) if not self.sm_is_left: self.ultrasonic_sensor_values.reverse() self.color_sensor_values.reverse() self.sm_is_left = not self.sm_is_left def directions_free(self) -> List[bool]: ''' Returns list of bools (left, center, right), representing if the directions are free to move. ''' return [a == 0 for a in self.color_sensor_values] def ghost_distance(self) -> List[int]: ''' Returns list of ints (left, center, right), representing the distance from closest ghost. ''' return [ int(a) // self.ultrasonic_tile_length if a < self.ultrasonic_max_value and a // self.ultrasonic_tile_length > 0 else None for a in self.ultrasonic_sensor_values ]
class Printer: colors = ('unknown', 'black', 'blue', 'green', 'yellow', 'red', 'white', 'brown') def __init__(self, pixel_size): self._touch = TouchSensor(INPUT_1) self._color = ColorSensor(INPUT_4) self._color.mode = 'COL-COLOR' self._fb_motor = LargeMotor(OUTPUT_C) self._lr_motor = LargeMotor(OUTPUT_B) self._ud_motor = Motor(OUTPUT_A) self._x_res = utilities.MAX_X_RES / int(pixel_size) self._y_res = utilities.MAX_Y_RES / int(pixel_size) self._padding_left = utilities.MAX_PADDING_LEFT / int(pixel_size) self._padding_right = utilities.MAX_PADDING_RIGHT / int(pixel_size) self._is_pen_up = True self._pen_calibrated = False self._ud_ratio = 5 self._fb_ratio = 4 self._lr_ratio = 1 self._pen_up_val = -3 * self._ud_ratio self._pen_down_val = -self._pen_up_val self._pen_up_down_speed = 10 self._pen_left_speed = 20 self._pen_right_speed = 20 self._paper_scroll_speed = 20 self._pixel_size = pixel_size self._p_codes = [] self._current_line = 0 def _pen_up(self, val): print("{} {}".format('PEN_UP', val)) if val > 0: self._ud_motor.on_for_degrees(self._pen_up_down_speed, val * self._pen_up_val) self._is_pen_up = True def _pen_down(self, val): print("{} {}".format('PEN_DOWN', val)) if val > 0: self._ud_motor.on_for_degrees(self._pen_up_down_speed, val * self._pen_down_val) self._is_pen_up = False def _pen_left(self, val): print("{} {}".format('PEN_LEFT', val)) if val > 0: self._lr_motor.on_for_degrees( self._pen_left_speed, int(self._pixel_size) * val * self._lr_ratio) def _pen_right(self, val): print("{} {}".format('PEN_RIGHT', val)) if val > 0: self._lr_motor.on_for_degrees( self._pen_right_speed, -int(self._pixel_size) * val * self._lr_ratio) def _paper_scroll(self, val): print("{} {}".format('SCROLL', val)) if val > 0: if val == 1: self._current_line += val print("-------------- Line {} --------------".format( self._current_line)) self._fb_motor.on_for_degrees( self._paper_scroll_speed, int(self._pixel_size) * val * self._fb_ratio) def _finish_calibration(self): self._pen_calibrated = True def calibrate(self, quick_calibration): speaker = Sound() if quick_calibration: speaker.speak("Quick calibration") print("Quick calibration...") else: speaker.speak("Calibrating") print("Calibrating...") btn = Button() self._lr_motor.reset() self._ud_motor.reset() self._fb_motor.reset() self._lr_motor.on_for_degrees( self._pen_left_speed, self._x_res * self._pixel_size * self._lr_ratio) self._lr_motor.reset() self._ud_motor.on(-15) self._touch.wait_for_pressed() self._ud_motor.stop() time.sleep(1) self._ud_motor.on(15) self._touch.wait_for_released() self._ud_motor.on_for_degrees(10, 40) self._ud_motor.stop() time.sleep(1) speaker.speak( "Insert calibration paper and press the touch sensor") self._touch.wait_for_pressed() speaker.speak("Adjust pen height") print("Adjust pen height...") while not self._pen_calibrated: self._lr_motor.on_for_degrees( self._pen_right_speed, -100 * self._pixel_size * self._lr_ratio) self._lr_motor.on_for_degrees( self._pen_left_speed, 100 * self._pixel_size * self._lr_ratio) time.sleep(1) if btn.up: self._pen_up(1) elif btn.down: self._pen_down(1) elif btn.right: self._finish_calibration() elif btn.left: self._pen_down(4) self._lr_motor.reset() if not self._is_pen_up: self._pen_up(1) self._pen_left(self._x_res) for _ in range(2): self._pen_right(self._x_res) self._pen_left(self._x_res) for _ in range(8): self._pen_right(self._padding_left) for _ in range(int(self._x_res)): self._pen_right(1) self._pen_left(self._x_res + self._padding_left) self._lr_motor.reset() speaker.speak( "Insert a blank piece of paper and press the touch sensor") self._touch.wait_for_pressed() self._fb_motor.on(5) val = 0 print("Scrolling the piece of paper to its starting position...") while self.colors[val] == 'unknown': val = self._color.value() self._fb_motor.reset() print("Calibration done") def _interpret_p_codes(self, p_codes): btn = Button() speaker = Sound() self._current_line = 0 abort = False print("---------- p_codes:----------") print("-------------- Line {} --------------".format( self._current_line)) for x in p_codes: if btn.down: speaker.speak("Aborting") print("\nAborting...") abort = True break if x[0] == utilities.Command.PEN_UP: if not self._is_pen_up: self._pen_up(x[1]) elif x[0] == utilities.Command.PEN_DOWN: if self._is_pen_up: self._pen_down(x[1]) elif x[0] == utilities.Command.PEN_RIGHT: self._pen_right(x[1]) elif x[0] == utilities.Command.PEN_LEFT: self._pen_left(x[1]) elif x[0] == utilities.Command.SCROLL: self._paper_scroll(x[1]) if not self._is_pen_up: self._pen_up(1) self._ud_motor.stop() return abort def draw(self, path=None, multicolor=False): speaker = Sound() if path is not None: if multicolor: speaker.speak("Printing multi color image") print("\nPrinting multi color image...") else: speaker.speak("Printing single color image") print("\nPrinting single color image...") binarized, img_x, img_y = utilities.binarize_image( path, self._x_res, self._y_res, multicolor) else: binarized, img_x, img_y = utilities.generate_and_binarize_test_image( self._pixel_size) speaker.speak("Printing test page") print("\nPrinting test page...") quick_calibration = False for layer, i in zip(binarized, range(1, utilities.NUM_OF_COLORS)): speaker.speak("Insert a {} pen and press the touch sensor".format( utilities.palette_names[i])) self._touch.wait_for_pressed() self.calibrate(quick_calibration) p_codes = utilities.binarized_image_to_p_codes( layer, img_x, img_y, self._padding_left) if self._interpret_p_codes(p_codes): break self._paper_scroll(self._y_res) self._fb_motor.reset() self._ud_motor.reset() self._lr_motor.reset() quick_calibration = True speaker = Sound() speaker.speak("Printing finished") print("Printing finished")