def test_stop_with_no_speed(self):
motor = Motor(OUTPUT_A)
self.clientSocketMock.send_command.return_value = 0
motor.on(0, False)
motor.stop()
self.assertEqual(len(self.clientSocketMock.mock_calls), 1)
self.assertEqual(motor.speed_sp, 0)
def reset():
fb_motor = LargeMotor(OUTPUT_C)
lr_motor = LargeMotor(OUTPUT_B)
ud_motor = Motor(OUTPUT_A)
fb_motor.stop()
lr_motor.stop()
ud_motor.stop()
def stopAll(self):
self.tank.off()
try:
for out in self.outputList:
m = Motor(out)
m.stop()
except:
pass
def test_stop_reverse(self):
motor = Motor(OUTPUT_A)
self.clientSocketMock.send_command.return_value = 0
motor.on(-10, False)
motor.stop()
self.assertEqual(len(self.clientSocketMock.mock_calls), 2)
fn_name, args, kwargs = self.clientSocketMock.mock_calls[1]
self.assertEqual(fn_name, 'send_command')
self.assertDictEqual(args[0].serialize(),
{'type': 'StopCommand', 'address': 'ev3-ports:outA', 'stop_action': 'coast',
'speed': 105})
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 test_direct_on_and_stop(self):
self.clientSocketMock.send_command.return_value = 1
motor = Motor(OUTPUT_A)
motor.on(10)
self.assertEqual(len(self.clientSocketMock.mock_calls), 1)
fn_name, args, kwargs = self.clientSocketMock.mock_calls[0]
self.assertEqual(fn_name, 'send_command')
distance = 105 * 45
self.assertDictEqual(args[0].serialize(),
{'type': 'RotateCommand', 'address': 'ev3-ports:outA', 'stop_action': 'hold',
'speed': 105, 'distance': distance})
motor.stop()
self.assertEqual(len(self.clientSocketMock.mock_calls), 2)
fn_name, args, kwargs = self.clientSocketMock.mock_calls[1]
self.assertEqual(fn_name, 'send_command')
self.assertDictEqual(args[0].serialize(),
{'type': 'StopCommand', 'address': 'ev3-ports:outA', 'stop_action': 'hold',
'speed': 105})
while event:
try:
(tv_sec, tv_usec, ev_type, code, value) = struct.unpack(FORMAT, event)
if ev_type == 3 and code == 3:
right_stick_x = value
if ev_type == 3 and code == 4:
right_stick_y = value
# Scale stick positions to -100,100
forward = scale(right_stick_y, (0, 255), (100, -100))
left = scale(right_stick_x, (0, 255), (100, -100))
# Set motor voltages. If we're steering left, the left motor
# must run backwards so it has a -left component
# It has a forward component for going forward too.
left_motor.run_direct(duty_cycle_sp=clamp(forward - left * 0.3))
right_motor.run_direct(duty_cycle_sp=clamp(forward + left * 0.3))
steer_err = steer_motor.position - left * 0.5
steer_motor.run_direct(duty_cycle_sp=clamp(steer_err * -2))
# Finally, read another event
event = in_file.read(EVENT_SIZE)
except:
in_file.close()
left_motor.stop()
right_motor.stop()
steer_motor.stop()
break
# long int, long int, unsigned short, unsigned short, unsigned int
FORMAT = 'llHHI'
EVENT_SIZE = struct.calcsize(FORMAT)
event = in_file.read(EVENT_SIZE)
while event:
try:
(tv_sec, tv_usec, ev_type, code, value) = struct.unpack(FORMAT, event)
if ev_type == 3 and code == 3:
right_stick_x = value
if ev_type == 3 and code == 4:
right_stick_y = value
# Scale stick positions to -100,100
forward = scale(right_stick_y, (0,255), (100,-100))
left = scale(right_stick_x, (0,255), (100,-100))
# Set motor voltages. If we're steering left, the left motor
# must run backwards so it has a -left component
# It has a forward component for going forward too.
left_motor.run_direct(duty_cycle_sp=clamp(forward - left))
right_motor.run_direct(duty_cycle_sp=clamp(forward + left))
# Finally, read another event
event = in_file.read(EVENT_SIZE)
except:
in_file.close()
left_motor.stop()
right_motor.stop()
break
class MindstormsGadget(AlexaGadget):
"""
A Mindstorms gadget that performs movement based on voice commands.
Two types of commands are supported, directional movement and preset.
"""
def __init__(self):
"""
Performs Alexa Gadget initialization routines and ev3dev resource allocation.
"""
super().__init__()
# Ev3dev initialization
self.leds = Leds()
self.sound = Sound()
self.motor = Motor(OUTPUT_A)
def on_connected(self, device_addr):
"""
Gadget connected to the paired Echo device.
:param device_addr: the address of the device we connected to
"""
self.leds.set_color("LEFT", "GREEN")
self.leds.set_color("RIGHT", "GREEN")
print("{} connected to Echo device".format(self.friendly_name))
def on_disconnected(self, device_addr):
"""
Gadget disconnected from the paired Echo device.
:param device_addr: the address of the device we disconnected from
"""
self.leds.set_color("LEFT", "BLACK")
self.leds.set_color("RIGHT", "BLACK")
print("{} disconnected from Echo device".format(self.friendly_name))
def on_custom_mindstorms_gadget_control(self, directive):
"""
Handles the Custom.Mindstorms.Gadget control directive.
:param directive: the custom directive with the matching namespace and name
"""
try:
payload = json.loads(directive.payload.decode("utf-8"))
print("Control payload: {}".format(payload))
control_type = payload["type"]
if control_type == "move":
# Expected params: [direction, duration, speed]
self._move(payload["direction"], int(payload["duration"]),
int(payload["speed"]))
if control_type == "command":
# Expected params: [command]
self._activate(payload["command"])
except KeyError:
print("Missing expected parameters: {}".format(directive))
def _move(self, direction, duration: int, speed: int, is_blocking=False):
"""
Handles move commands from the directive.
Right and left movement can under or over turn depending on the surface type.
:param direction: the move direction
:param duration: the duration in rotations
:param speed: the speed percentage as an integer
:param is_blocking: if set, motor run until duration expired before accepting another command
"""
print("Move command: ({}, {}, {}, {})".format(direction, speed,
duration, is_blocking))
position = duration * 360
if direction in Direction.FORWARD.value:
self.motor.run_to_rel_pos(position_sp=(position),
speed_sp=(speed),
stop_action="hold")
if direction in Direction.BACKWARD.value:
self.motor.run_to_rel_pos(position_sp=(-position),
speed_sp=(speed),
stop_action="hold")
if direction in Direction.STOP.value:
self.motor.stop(stop_action="hold")
def _activate(self, command, speed=500):
"""
Handles preset commands.
:param command: the preset command
:param speed: the speed if applicable
"""
print("Activate command: ({}, {})".format(command, speed))
if command in Command.RAISE_BLIND.value:
self.motor.run_to_rel_pos(position_sp=5000,
speed_sp=500,
stop_action="hold")
print('running raise blinds')
if command in Command.LOWER_BLIND.value:
self.motor.run_to_rel_pos(position_sp=-5000,
speed_sp=500,
stop_action="hold")
print('running lower blinds')
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")
