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")
