def test_single_motor(output):
motor = LargeMotor(output)
# motor.command = LargeMotor.COMMAND_RUN_FOREVER
# for command in motor.commands:
# print('Motor at {} set to {}'.format(output, command))
# motor.command = command
motor.on_for_seconds(SpeedPercent(100), 5)
# print_and_wait(motor)
motor.on_for_rotations(SpeedPercent(30), 0.5)
# print_and_wait(motor)
motor.on_for_degrees(SpeedPercent(30), 45)
# print_and_wait(motor)
motor.on_to_position(SpeedPercent(30), 5)
#!/usr/bin/env python3 from ev3dev2.motor import LargeMotor from time import sleep lm = LargeMotor() lm.on(speed=50, brake=True, block=False) sleep(1) lm.off() sleep(1) lm.on_for_seconds(speed=50, seconds=2, brake=True, block=True) sleep(1) lm.on_for_rotations(50, 3) sleep(1) lm.on_for_degrees(50, 90) sleep(1) lm.on_to_position(50, 180) sleep(1)
#
import time
from ev3dev2.motor import LargeMotor, OUTPUT_A, OUTPUT_B, SpeedPercent, SpeedDPS, MoveTank
from ev3dev2.sensor import INPUT_1
print("Please attach a motor to BAM1")
time.sleep(3)
start_pos = 0
end_pos = 0
start_time = 0.0
end_time = 0.0
m = LargeMotor(OUTPUT_A)
time.sleep(0.1)
m.reset()
time.sleep(0.1)
start_time = time.time()
for i in range(1, 11):
m.on_to_position(SpeedDPS(600), (i * 360), brake=False,
block=True) ### this method FAILS TO BLOCK
# m.on_for_degrees(SpeedDPS(600), (i * 360), brake=False, block=True) ### this method FAILS TO BLOCK
# time.sleep(.1)
m.wait_while('running')
m.off(brake=True)
print("target pos = ", (i * 360), "; actual pos = ", m.position)
m.reset()
time.sleep(.1)
class Rollers:
"""Rollers class witch allow managing paper. """
def __init__(self, power=30, prevent_paper_blocking=False):
_debug(self, "Creating Rollers instance")
self._default_power = power
self._delta_in = 0
self._delta_out = 0
self._in = LargeMotor(OUTPUT_A)
self._in.polarity = LargeMotor.POLARITY_NORMAL
self._out = LargeMotor(OUTPUT_D)
self._out.polarity = LargeMotor.POLARITY_INVERSED
self._col = ColorSensor()
self._col.mode = ColorSensor.MODE_COL_COLOR
self._paper_taken = self._col.color == 6
self.reset(prevent_paper_blocking)
def reset(self, prevent_paper_blocking=False, power=None):
"""Eject paper if present and prevent paper blocking
:param prevent_paper_blocking: if true, roller will move to avoid paper blocking
:param power: Power used to move rollers
"""
if power is None:
power = self._default_power
if self._paper_taken:
self.eject_paper()
elif prevent_paper_blocking:
self._in.on_for_rotations(power, 3, block=False)
self._out.on_for_rotations(power, 3)
def up(self, power=None):
"""Move paper to the 'up'
:param power: Power of the rotation
"""
if power is None:
power = self._default_power
self._in.on(power)
self._out.on(power)
def down(self, power=None):
"""Move paper to the 'down'
:param power: Power of the rotation
"""
if power is None:
power = self._default_power
self._in.on(-power)
self._out.on(-power)
def stop(self):
""" Stop moving rollers
"""
self._in.off()
self._out.off()
def save_energy(self):
"""Save energy and release motor's holding state"""
self._in.off(False)
self._out.off(False)
@property
def has_paper(self):
""" Get the paper state
:return True if paper present"""
return self._paper_taken
@property
def position(self):
""" Get rollers position
:return: rollers position
"""
if not self.has_paper:
return None
return self._in.position - self._delta_in, self._out.position - self._delta_out
@position.setter
def position(self, value):
""" Set Rollers position
:param value: position reached"""
self.go_to(value)
def go_to(self, position, power=None, block=True, override=False):
"""Go to absolute position `position`
:param position: Position Reached
:param power: Power used to move
:param block: If True, fonction will end at the end of the rotation
:param override: if true, bypass limits"""
if not self.has_paper:
raise ValueError("There is no paper.")
if power is None:
power = self._default_power
target_in = self._delta_in + position
target_out = self._delta_out + position
_debug(self, "Reached position is {}".format(position))
_debug(self, "Target In {}".format(target_in))
_debug(self, "Target Out {}".format(target_out))
if (not override) and (not 0 <= position <= 515):
raise ValueError("Position is out of reachable bounds.")
self._in.on_to_position(power, target_in, block=False)
self._out.on_to_position(power, target_out, block=block)
def move(self, value, power=None, block=True):
"""Move rollers (and paper if present) of `value` degrees
:param value: Degrees to move
:param power; Power used to move
:param block: If True, fonction will end at the end of the rotation"""
if power is None:
power = self._default_power
self._in.on_to_position(power, self._in.position + value, block=False)
self._out.on_to_position(power, self._out.position + value, block=block)
def up_limit(self, power=None):
"""Go to paper up limit
:param power: Power used to move"""
self.go_to(0, power)
def down_limit(self, power=None):
"""Go to paper down limit
:param power: Power used to move"""
self.go_to(515, power)
def take_paper(self, power=None, power_grip=15):
""" Take paper into printer and stretch it
:param power: Power used to take paper
:param power_grip: Power used to stretch paper
"""
self.up(power)
while self._col.color != 6:
sleep(0.1)
self.stop()
self._out.on_for_seconds(power_grip, 1)
self._paper_taken = self._col.color == 6
self.move(-130)
sleep(0.2)
self._delta_in = self._in.position
self._delta_out = self._out.position
def eject_paper(self, power=None):
""" Eject paper
:param power: Power used to eject paper
"""
self.up(power)
sleep(0.5)
while self._col.color == 6:
sleep(0.1)
sleep(0.5)
self.stop()
self._paper_taken = False
self._delta_in = 0
self._delta_out = 0
@property
def default_power(self):
return self._default_power
@default_power.setter
def default_power(self, value):
self._default_power = value
class Pen:
"""Pen class witch allow managing the pen position. """
def __init__(self, power=20):
_debug(self, "Creating Pen instance")
self._default_power = power
self._pen_up_position = 0 # Lambda values. Needed to be set before !
self._pen_down_position = 40
self._m = LargeMotor(OUTPUT_C)
self._m.stop_action = LargeMotor.STOP_ACTION_HOLD
self.reset()
def up(self, power=None):
""" Set pen to 'up' position (only if is not already)
:param power: Power of the rotation
"""
if power is None:
power = self._default_power
if not self.is_up:
self._m.on_to_position(power, self._pen_up_position)
def down(self, power=None):
""" Set pen to 'down' position (only if is not already)
:param power: Power of the rotation
"""
if power is None:
power = self._default_power
if self.is_up:
self._m.on_to_position(power, self._pen_down_position)
@property
def is_up(self):
""" Get the pen position
:return: True if pen up, False otherwise
"""
return self._m.position < self._pen_up_position + 15
def toggle(self):
""" Change pen position to the opposite
Pen Up ==> Pen Down
Pen Down ==> Pen Up
"""
if self.is_up:
self.down()
else:
self.up()
def reset(self, power=None):
"""Reset the pen position (set it to 'up')
:param power: Power used to move the pen
"""
if power is None:
power = self._default_power
self._m.on(-power)
self._m.wait_until_not_moving()
self._m.off()
self._m.on_for_degrees(power, 20)
sleep(1)
def setup(self, validator):
""" Setup the pen, define up and down position.
:param validator: A callable objet used to validate the pen down position.
When `True` is returned, the pen position will be saved to the down position."""
self.reset()
self._pen_up_position = self._m.position
self._m.off(False)
while not validator():
sleep(0.1)
self._pen_down_position = self._m.position
self.up()
def save_energy(self):
"""Save energy and release motor's holding state"""
self._m.off(False)
class Carriage:
"""Carriage class witch allow managing the position. """
def __init__(self, power=30):
_debug(self, "Creating Carriage instance")
self._default_power = power
self._delta = 0
self._m = LargeMotor(OUTPUT_B)
self.reset()
def reset(self, power=None):
"""Reset the carriage position and define a new origin for carriage's position
:param power: Power used to move the carriage
"""
if power is None:
power = self._default_power
self.right_limit(power=power, soft_limit=False)
self._m.on_for_degrees(power, 50)
self._delta = self._m.position
self.go_to(0, power)
def left(self, power=None):
"""Move carriage (pen will move too) to the 'left'
:param power: Power of the rotation
"""
if power is None:
power = self._default_power
self._m.on(power)
def right(self, power=None):
"""Move carriage (pen will move too) to the 'right'
:param power: Power of the rotation
"""
if power is None:
power = self._default_power
self._m.on(-power)
def stop(self):
""" Stop moving carriage
"""
self._m.off()
@property
def position(self):
""" Get carriage position
:return: carriage position
"""
return self._m.position - self._delta
@position.setter
def position(self, value):
""" Set carriage position
:param value: Reached position"""
self.go_to(value)
def go_to(self, position, power=None, block=True, override=False):
"""Move carriage to absolute position `position`
:param position: Position reached
:param power: Power used to move
:param block: If True, fonction will end at the end of the rotation
:param override: if true, bypass limits"""
if power is None:
power = self._default_power
_debug(self, "Reached position is {}".format(position))
if (not override) and (not -50 <= position <= 1240):
raise ValueError("Position is out of reachable bounds.")
self._m.on_to_position(power, position + self._delta, block=block)
def move(self, value, power=None, block=True, override=False):
"""Move carriage of `value` degrees
:param value: Position reached
:param power: Power used to move
:param block: If True, fonction will end at the end of the rotation
:param override: if true, bypass limits"""
if power is None:
power = self._default_power
self.go_to(self.position + value, power, block, override)
def save_energy(self):
"""Save energy and release motor's holding state"""
self._m.off(False)
def right_limit(self, soft_limit=True, power=None):
"""Go to the right limit
:param soft_limit: If True, carriage will move backward to avoid motor forcing
:param power: power used to move"""
if power is None:
power = self._default_power
self.right(power)
self._m.wait_until_not_moving()
self.stop()
if soft_limit:
self.move(50, power)
def left_limit(self, soft_limit=True, power=None):
"""Go to the left limit
:param soft_limit: If True, carriage will move backward to avoid motor forcing
:param power: power used to move"""
if power is None:
power = self._default_power
self.left(power)
self._m.wait_until_not_moving()
self.stop()
if soft_limit:
self.move(-50, power)
@property
def default_power(self):
return self._default_power
@default_power.setter
def default_power(self, value):
self._default_power = value
m1 = LargeMotor(OUTPUT_A)
m2 = LargeMotor(OUTPUT_B)
m3 = LargeMotor(OUTPUT_C)
print("ready!")
ts.wait_for_bump() # wait for user to calibrate motors
m1.reset()
m2.reset()
m3.reset()
while True:
ts.wait_for_bump()
m1.on(speed=randint(-100, 100))
m2.on(speed=randint(-100, 100))
m3.on(speed=randint(-100, 100))
ts.wait_for_bump()
m1.off(brake=False)
ts.wait_for_bump()
m2.off(brake=False)
ts.wait_for_bump()
m3.off(brake=False)
ts.wait_for_bump()
pos1 = round(m1.position / 45) * 45
pos2 = round(m2.position / 45) * 45
pos3 = round(m3.position / 45) * 45
m1.on_to_position(100, pos1)
m2.on_to_position(100, pos2)
m3.on_to_position(100, pos3)
grabber_open = False
grabber_close = True
else:
grabber_open = True
grabber_close = False
if event.type == 1 and event.code == 314 and event.value == 1: #Share
#Demo
if event.type == 1 and event.code == 315 and event.value == 1: #Options
#Reset
roll_motor.on_to_position(normal_speed,0,True,False)
pitch_motor.on_to_position(normal_speed,0,True,False)
spin_motor.on_to_position(normal_speed,0,True,False)
grabber_motor.on_to_position(normal_speed,0,True,True)
elbow_motor.on_to_position(slow_speed,0,True,False)
shoulder_control1.on_to_position(slow_speed,0,True,False)
shoulder_control2.on_to_position(slow_speed,0,True,False)
waist_motor.on_to_position(fast_speed,0,True,True)
if event.type == 1 and event.code == 316 and event.value == 1: #PS
logger.info("Engine stopping!")
running = False
#Reset
roll_motor.on_to_position(normal_speed,0,True,False)
pitch_motor.on_to_position(normal_speed,0,True,False)
spin_motor.on_to_position(normal_speed,0,True,False)
grabber_motor.on_to_position(normal_speed,0,True,True)
elbow_motor.on_to_position(slow_speed,0,True,False)
shoulder_control1.on_to_position(slow_speed,0,True,False)
valChange = 0
dgrs = 30
for i in range(10):
print("iteration = ", i)
startTime = time.time()
large_motor.on_for_degrees(speed=mAspeed, degrees=dgrs, block=False)
while abs(large_motor.duty_cycle) <= abs(abs(mAspeed) + abs(duty_cycle_margin)):
duty_cycle_avg = (duty_cycle_avg + large_motor.duty_cycle) / 2
duty_cycle_min = min(duty_cycle_min, large_motor.duty_cycle)
duty_cycle_max = max(duty_cycle_max, large_motor.duty_cycle)
if valChange != duty_cycle_min + duty_cycle_max:
print("position, duty_cycle avg, min, max = ", large_motor.position, duty_cycle_avg, duty_cycle_min, duty_cycle_max)
valChange = duty_cycle_min + duty_cycle_max
time.sleep(0.01)
if time.time() - startTime > 2:
print("timeout - exiting")
break
print("position, duty_cycle avg, min, max = ", large_motor.position, duty_cycle_avg, duty_cycle_min, duty_cycle_max)
large_motor.on_to_position(-1 * (mAspeed/4), 0)
large_motor.off(brake=False)
time.sleep(0.1)
print("motor should now be stopped")
class MindCuber(object):
scan_order = [
5, 9, 6, 3, 2, 1, 4, 7, 8, 23, 27, 24, 21, 20, 19, 22, 25, 26, 50, 54,
51, 48, 47, 46, 49, 52, 53, 14, 10, 13, 16, 17, 18, 15, 12, 11, 41, 43,
44, 45, 42, 39, 38, 37, 40, 32, 34, 35, 36, 33, 30, 29, 28, 31
]
hold_cube_pos = 85
rotate_speed = 400
flip_speed = 450
flip_speed_push = 400
def __init__(self):
self.shutdown = False
self.flipper = LargeMotor(OUTPUT_A)
self.turntable = LargeMotor(OUTPUT_B)
self.colorarm = MediumMotor(OUTPUT_C)
self.color_sensor = ColorSensor()
self.color_sensor.mode = self.color_sensor.MODE_RGB_RAW
self.infrared_sensor = InfraredSensor()
self.init_motors()
self.state = ['U', 'D', 'F', 'L', 'B', 'R']
self.rgb_solver = None
signal.signal(signal.SIGTERM, self.signal_term_handler)
signal.signal(signal.SIGINT, self.signal_int_handler)
filename_max_rgb = 'max_rgb.txt'
if os.path.exists(filename_max_rgb):
with open(filename_max_rgb, 'r') as fh:
for line in fh:
(color, value) = line.strip().split()
if color == 'red':
self.color_sensor.red_max = int(value)
log.info("red max is %d" % self.color_sensor.red_max)
elif color == 'green':
self.color_sensor.green_max = int(value)
log.info("green max is %d" %
self.color_sensor.green_max)
elif color == 'blue':
self.color_sensor.blue_max = int(value)
log.info("blue max is %d" % self.color_sensor.blue_max)
def init_motors(self):
for x in (self.flipper, self.turntable, self.colorarm):
x.reset()
log.info("Initialize flipper %s" % self.flipper)
self.flipper.on(SpeedDPS(-50), block=True)
self.flipper.off()
self.flipper.reset()
log.info("Initialize colorarm %s" % self.colorarm)
self.colorarm.on(SpeedDPS(500), block=True)
self.colorarm.off()
self.colorarm.reset()
log.info("Initialize turntable %s" % self.turntable)
self.turntable.off()
self.turntable.reset()
def shutdown_robot(self):
log.info('Shutting down')
self.shutdown = True
if self.rgb_solver:
self.rgb_solver.shutdown = True
for x in (self.flipper, self.turntable, self.colorarm):
# We are shutting down so do not 'hold' the motors
x.stop_action = 'brake'
x.off(False)
def signal_term_handler(self, signal, frame):
log.error('Caught SIGTERM')
self.shutdown_robot()
def signal_int_handler(self, signal, frame):
log.error('Caught SIGINT')
self.shutdown_robot()
def apply_transformation(self, transformation):
self.state = [self.state[t] for t in transformation]
def rotate_cube(self, direction, nb):
current_pos = self.turntable.position
final_pos = 135 * round(
(self.turntable.position + (270 * direction * nb)) / 135.0)
log.info(
"rotate_cube() direction %s, nb %s, current_pos %d, final_pos %d" %
(direction, nb, current_pos, final_pos))
if self.flipper.position > 35:
self.flipper_away()
self.turntable.on_to_position(SpeedDPS(MindCuber.rotate_speed),
final_pos)
if nb >= 1:
for i in range(nb):
if direction > 0:
transformation = [0, 1, 5, 2, 3, 4]
else:
transformation = [0, 1, 3, 4, 5, 2]
self.apply_transformation(transformation)
def rotate_cube_1(self):
self.rotate_cube(1, 1)
def rotate_cube_2(self):
self.rotate_cube(1, 2)
def rotate_cube_3(self):
self.rotate_cube(-1, 1)
def rotate_cube_blocked(self, direction, nb):
# Move the arm down to hold the cube in place
self.flipper_hold_cube()
# OVERROTATE depends on lot on MindCuber.rotate_speed
current_pos = self.turntable.position
OVERROTATE = 18
final_pos = int(135 * round(
(current_pos + (270 * direction * nb)) / 135.0))
temp_pos = int(final_pos + (OVERROTATE * direction))
log.info(
"rotate_cube_blocked() direction %s nb %s, current pos %s, temp pos %s, final pos %s"
% (direction, nb, current_pos, temp_pos, final_pos))
self.turntable.on_to_position(SpeedDPS(MindCuber.rotate_speed),
temp_pos)
self.turntable.on_to_position(SpeedDPS(MindCuber.rotate_speed / 4),
final_pos)
def rotate_cube_blocked_1(self):
self.rotate_cube_blocked(1, 1)
def rotate_cube_blocked_2(self):
self.rotate_cube_blocked(1, 2)
def rotate_cube_blocked_3(self):
self.rotate_cube_blocked(-1, 1)
def flipper_hold_cube(self, speed=300):
current_position = self.flipper.position
# Push it forward so the cube is always in the same position
# when we start the flip
if (current_position <= MindCuber.hold_cube_pos - 10
or current_position >= MindCuber.hold_cube_pos + 10):
self.flipper.ramp_down_sp = 400
self.flipper.on_to_position(SpeedDPS(speed),
MindCuber.hold_cube_pos)
sleep(0.05)
def flipper_away(self, speed=300):
"""
Move the flipper arm out of the way
"""
log.info("flipper_away()")
self.flipper.ramp_down_sp = 400
self.flipper.on_to_position(SpeedDPS(speed), 0)
def flip(self):
"""
Motors will sometimes stall if you call on_to_position() multiple
times back to back on the same motor. To avoid this we call a 50ms
sleep in flipper_hold_cube() and after each on_to_position() below.
We have to sleep after the 2nd on_to_position() because sometimes
flip() is called back to back.
"""
log.info("flip()")
if self.shutdown:
return
# Move the arm down to hold the cube in place
self.flipper_hold_cube()
# Grab the cube and pull back
self.flipper.ramp_up_sp = 200
self.flipper.ramp_down_sp = 0
self.flipper.on_to_position(SpeedDPS(self.flip_speed), 190)
sleep(0.05)
# At this point the cube is at an angle, push it forward to
# drop it back down in the turntable
self.flipper.ramp_up_sp = 200
self.flipper.ramp_down_sp = 400
self.flipper.on_to_position(SpeedDPS(self.flip_speed_push),
MindCuber.hold_cube_pos)
sleep(0.05)
transformation = [2, 4, 1, 3, 0, 5]
self.apply_transformation(transformation)
def colorarm_middle(self):
log.info("colorarm_middle()")
self.colorarm.on_to_position(SpeedDPS(600), -750)
def colorarm_corner(self, square_index):
"""
The lower the number the closer to the center
"""
log.info("colorarm_corner(%d)" % square_index)
position_target = -580
if square_index == 1:
position_target -= 10
elif square_index == 3:
position_target -= 30
elif square_index == 5:
position_target -= 20
elif square_index == 7:
pass
else:
raise ScanError(
"colorarm_corner was given unsupported square_index %d" %
square_index)
self.colorarm.on_to_position(SpeedDPS(600), position_target)
def colorarm_edge(self, square_index):
"""
The lower the number the closer to the center
"""
log.info("colorarm_edge(%d)" % square_index)
position_target = -640
if square_index == 2:
position_target -= 20
elif square_index == 4:
position_target -= 40
elif square_index == 6:
position_target -= 20
elif square_index == 8:
pass
else:
raise ScanError(
"colorarm_edge was given unsupported square_index %d" %
square_index)
self.colorarm.on_to_position(SpeedDPS(600), position_target)
def colorarm_remove(self):
log.info("colorarm_remove()")
self.colorarm.on_to_position(SpeedDPS(600), 0)
def colorarm_remove_halfway(self):
log.info("colorarm_remove_halfway()")
self.colorarm.on_to_position(SpeedDPS(600), -400)
def scan_face(self, face_number):
log.info("scan_face() %d/6" % face_number)
if self.shutdown:
return
if self.flipper.position > 35:
self.flipper_away(100)
self.colorarm_middle()
self.colors[int(MindCuber.scan_order[self.k])] = self.color_sensor.rgb
self.k += 1
i = 1
target_pos = 115
self.colorarm_corner(i)
# The gear ratio is 3:1 so 1080 is one full rotation
self.turntable.reset()
self.turntable.on_to_position(SpeedDPS(MindCuber.rotate_speed),
1080,
block=False)
self.turntable.wait_until('running')
while True:
# 135 is 1/8 of full rotation
if self.turntable.position >= target_pos:
current_color = self.color_sensor.rgb
self.colors[int(MindCuber.scan_order[self.k])] = current_color
i += 1
self.k += 1
if i == 9:
# Last face, move the color arm all the way out of the way
if face_number == 6:
self.colorarm_remove()
# Move the color arm far enough away so that the flipper
# arm doesn't hit it
else:
self.colorarm_remove_halfway()
break
elif i % 2:
self.colorarm_corner(i)
if i == 1:
target_pos = 115
elif i == 3:
target_pos = 380
else:
target_pos = i * 135
else:
self.colorarm_edge(i)
if i == 2:
target_pos = 220
elif i == 8:
target_pos = 1060
else:
target_pos = i * 135
if self.shutdown:
return
if i < 9:
raise ScanError('i is %d..should be 9' % i)
self.turntable.wait_until_not_moving()
self.turntable.off()
self.turntable.reset()
log.info("\n")
def scan(self):
log.info("scan()")
self.colors = {}
self.k = 0
self.scan_face(1)
self.flip()
self.scan_face(2)
self.flip()
self.scan_face(3)
self.rotate_cube(-1, 1)
self.flip()
self.scan_face(4)
self.rotate_cube(1, 1)
self.flip()
self.scan_face(5)
self.flip()
self.scan_face(6)
if self.shutdown:
return
log.info("RGB json:\n%s\n" % json.dumps(self.colors))
self.rgb_solver = RubiksColorSolverGeneric(3)
self.rgb_solver.enter_scan_data(self.colors)
self.rgb_solver.crunch_colors()
self.cube_kociemba = self.rgb_solver.cube_for_kociemba_strict()
log.info("Final Colors (kociemba): %s" % ''.join(self.cube_kociemba))
# This is only used if you want to rotate the cube so U is on top, F is
# in the front, etc. You would do this if you were troubleshooting color
# detection and you want to pause to compare the color pattern on the
# cube vs. what we think the color pattern is.
'''
log.info("Position the cube so that U is on top, F is in the front, etc...to make debugging easier")
self.rotate_cube(-1, 1)
self.flip()
self.flipper_away()
self.rotate_cube(1, 1)
input('Paused')
'''
def move(self, face_down):
log.info("move() face_down %s" % face_down)
position = self.state.index(face_down)
actions = {
0: ["flip", "flip"],
1: [],
2: ["rotate_cube_2", "flip"],
3: ["rotate_cube_1", "flip"],
4: ["flip"],
5: ["rotate_cube_3", "flip"]
}.get(position, None)
for a in actions:
if self.shutdown:
break
getattr(self, a)()
def run_kociemba_actions(self, actions):
log.info('Action (kociemba): %s' % ' '.join(actions))
total_actions = len(actions)
for (i, a) in enumerate(actions):
if self.shutdown:
break
if a.endswith("'"):
face_down = list(a)[0]
rotation_dir = 1
elif a.endswith("2"):
face_down = list(a)[0]
rotation_dir = 2
else:
face_down = a
rotation_dir = 3
log.info("Move %d/%d: %s%s (a %s)" %
(i, total_actions, face_down, rotation_dir, pformat(a)))
self.move(face_down)
if rotation_dir == 1:
self.rotate_cube_blocked_1()
elif rotation_dir == 2:
self.rotate_cube_blocked_2()
elif rotation_dir == 3:
self.rotate_cube_blocked_3()
log.info("\n")
def resolve(self):
if self.shutdown:
return
cmd = ['kociemba', ''.join(map(str, self.cube_kociemba))]
output = check_output(cmd).decode('ascii')
if 'ERROR' in output:
msg = "'%s' returned the following error\n%s\n" % (' '.join(cmd),
output)
log.error(msg)
print(msg)
sys.exit(1)
actions = output.strip().split()
self.run_kociemba_actions(actions)
self.cube_done()
def cube_done(self):
self.flipper_away()
def wait_for_cube_insert(self):
rubiks_present = 0
rubiks_present_target = 10
log.info('wait for cube...to be inserted')
while True:
if self.shutdown:
break
dist = self.infrared_sensor.proximity
# It is odd but sometimes when the cube is inserted
# the IR sensor returns a value of 100...most of the
# time it is just a value less than 50
if dist < 50 or dist == 100:
rubiks_present += 1
log.info("wait for cube...distance %d, present for %d/%d" %
(dist, rubiks_present, rubiks_present_target))
else:
if rubiks_present:
log.info('wait for cube...cube removed (%d)' % dist)
rubiks_present = 0
if rubiks_present >= rubiks_present_target:
log.info('wait for cube...cube found and stable')
break
time.sleep(0.1)
class ALBERT(object):
def __init__(self):
# ev3dev initialization
self.leds = Leds()
self.sound = Sound()
self.arm_base = LargeMotor(OUTPUT_D)
self.arm_shoulder = LargeMotor(OUTPUT_C)
self.arm_wrist = LargeMotor(OUTPUT_B)
self.arm_gripper = MediumMotor(OUTPUT_A)
self.station = Workstation()
self.color_sensor = ColorSensor(INPUT_1)
self.find_indicator()
self.rotate_base(STATION_COLOR)
self.reset_all_motors()
def find_indicator(self):
''' Search for a valid color indicator. '''
if self.color_sensor.color in VALID_COLORS:
return
self.arm_base.on(10)
while self.color_sensor.color not in VALID_COLORS:
pass
self.arm_base.stop()
def reset_all_motors(self):
''' Reset all motor tach counts. '''
self.arm_base.reset()
self.arm_shoulder.reset()
self.arm_wrist.reset()
self.arm_gripper.reset()
def rotate_base(self, color):
'''
Rotate from one color indicator to another.
Color order is:
YELLOW <--> BLUE <--> RED
STORE <--> STATION <--> STERILE
'''
current_color = self.color_sensor.color
if current_color == color:
return
direction = 1
if (current_color == STATION_COLOR
and color == STERILE_COLOR) or current_color == STORE_COLOR:
direction = -1
self.arm_base.on(SPEEDS[0] * direction, block=False)
while self.color_sensor.color != color:
pass
self.arm_base.stop()
self.arm_base.on_for_rotations(SPEEDS[0], direction * STRIPE_BIAS)
def make_plate(self):
''' Sequence to make a plate. '''
self.get_plate()
self.lift_lid()
self.swab_plate()
self.lower_lid()
self.store_plate()
def check_plate(self):
''' Sequence to check plate. '''
self.get_plate(from_storage=True, upside_down=True)
self.move_to_keypoint(KP_UP_HORZ)
refl = self.station.check_status()
self.move_to_keypoint(KP_DOWN_HORZ)
self.store_plate(is_upside_down=True)
return refl
def get_plate(self, from_storage=False, upside_down=False):
'''
Sequence to get a plate and place it in the workstation.
Post-conditions
Gripper: WIDE
Arm: DOWN
Base: STATION
'''
src = STORE_COLOR if from_storage else STERILE_COLOR
self.move_to_keypoint(KP_UP_HORZ)
self.rotate_base(src)
self.set_gripper(GRIP_NARROW)
self.move_to_keypoint(KP_DOWN_HORZ)
self.set_gripper(GRIP_CLOSED)
self.move_to_keypoint(KP_UP_HORZ)
self.rotate_base(STATION_COLOR)
dest_up = KP_UP_VERT_INVERT if upside_down else KP_UP_VERT
dest_down = KP_DOWN_VERT_INVERT if upside_down else KP_DOWN_VERT
self.move_to_keypoint(dest_up)
self.move_to_keypoint(dest_down)
self.set_gripper(GRIP_WIDE)
self.move_to_keypoint(KP_DOWN_HORZ)
def lift_lid(self):
'''
Lift the dish lid.
Pre-condition
Gripper: WIDE
Arm: DOWN
Base: STATION
Post-condition
Gripper: CLOSED
Arm: UP
'''
self.move_to_keypoint(KP_DOWN_HORZ_LID)
self.set_gripper(GRIP_CLOSED)
self.move_to_keypoint(KP_UP_HORZ)
def lower_lid(self):
'''
Lower the dish lid.
Pre-condition
Gripper: CLOSED
Arm: UP
Base: STATION
Post-condition
Gripper: WIDE
Arm: DOWN
'''
self.move_to_keypoint(KP_DOWN_HORZ_LID)
self.set_gripper(GRIP_WIDE)
def swab_plate(self):
''' Call the Workstation swab routine. '''
self.station.swab()
def store_plate(self, is_upside_down=False):
''' Sequence to store a plate. '''
src_down = KP_DOWN_VERT_INVERT if is_upside_down else KP_DOWN_VERT
self.move_to_keypoint(src_down)
self.set_gripper(GRIP_CLOSED)
self.move_to_keypoint(KP_UP_HORZ)
self.rotate_base(STORE_COLOR)
self.move_to_keypoint(KP_DOWN_HORZ)
self.set_gripper(GRIP_NARROW)
self.move_to_keypoint(KP_UP_VERT)
self.rotate_base(STATION_COLOR)
self.set_gripper(GRIP_CLOSED)
def move_to_keypoint(self, keypoint):
''' Move the should/wrist to a keypoint.'''
# keypoint is [shoulder, wrist] with unit of rotations
self.arm_shoulder.on_to_position(
SPEEDS[1], keypoint[0] * self.arm_shoulder.count_per_rot)
self.arm_wrist.on_to_position(
SPEEDS[2], keypoint[1] * self.arm_wrist.count_per_rot)
# pause to let things settle
time.sleep(MOVE_PAUSE)
def set_gripper(self, position):
''' Set the gripper position. '''
self.arm_gripper.on_to_position(
25, self.arm_gripper.count_per_rot * position)
time.sleep(MOVE_PAUSE)
class MindstormsGadget(AlexaGadget):
"""
A Mindstorms gadget that performs movement based on voice commands.
"""
def __init__(self):
"""
Performs Alexa Gadget initialization routines and ev3dev resource allocation.
"""
super().__init__()
# Ev3dev initialization
self.leds = Leds()
self.sound = Sound()
self.drive = LargeMotor(OUTPUT_B)
self.dealmotor = MediumMotor(OUTPUT_A)
self.bcolor = ColorSensor(INPUT_1)
self.pushsensor = TouchSensor(INPUT_2)
self.leftmargin = 0
self.rigtmargin = 0
self._init_reset()
def _init_reset(self):
self.numCards = 2
self.numPlayers = 0
self.game = 'blackjack'
self.degreeStep = 180
self.players = ["red","yellow"] #Default player
self._send_event(EventName.SPEECH, {'speechOut': "Restart game"})
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")
logger.info("{} 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")
logger.info("{} 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), file=sys.stderr)
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"])
if control_type == "dealcard":
# Expected params: [command] = Number of Cards
# Expected params: [player]
num = payload["command"]
player = payload["player"]
speak = "Give "+ num + " card to " + player
if player == "all":
if (self.numPlayers == 0):
self.numPlayers = 2
for i in range(int(num)):
for j in range(self.numPlayers):
self._dealcard(1,j)
else:
try:
num = self.players.index(player)
self._dealcard(int(payload["command"]),num)
except ValueError:
speak = "There is no user " + player
print (speak,file=sys.stderr)
self._send_event(EventName.SPEECH, {'speechOut': speak})
except KeyError:
print("Missing expected parameters: {}".format(directive), file=sys.stderr)
def _dealcard(self, num, player):
"""
Deal number of cards to player
"""
if num < 1:
num = 1
degree = self._calcDegree(player)
print("Give player : {} card : {} Move angle : {}".format(player, num,degree), file=sys.stderr)
self.drive.on_to_position(SpeedPercent(10),degree)
self.drive.wait_until_not_moving()
for i in range(num):
self.dealmotor.on_for_degrees(SpeedPercent(-100), 340)
self.dealmotor.wait_until_not_moving()
time.sleep(1)
self.dealmotor.on_for_degrees(SpeedPercent(100), 270)
self.dealmotor.wait_until_not_moving()
time.sleep(1)
def _calcDegree (self,player):
degree = (player*self.degreeStep)+self.leftmargin
return degree
def _gameinit(self,game):
"""
Check and start game
"""
if (self.numPlayers == 0):
self.numPlayers = 2
if game == "poker":
self.numCards = 5
if game == "blackjack":
self.numCards = 2
if game == "rummy":
self.numCards = 7
if (self.numPlayers == 2):
self.numCards = 10
speak = ""
for i in range(self.numPlayers):
print("Player : {} Color : {}".format(i,self.players[i]), file=sys.stderr)
speak = speak + self.players[i]
speak = "Start " + game + "with " + str(self.numPlayers) + "player " + speak
self._send_event(EventName.SPEECH, {'speechOut': speak})
self._findboundary()
self.drive.on_to_position(SpeedPercent(10),self.leftmargin)
time.sleep(1)
print("Game : {} Number of Card : {}".format(game,self.numCards), file=sys.stderr)
for i in range(self.numCards):
for j in range(self.numPlayers):
self._dealcard(1,j)
def _findboundary (self):
"Move to left until sensor pressed "
self.drive.on(SpeedPercent(10))
self.pushsensor.wait_for_pressed ()
self.drive.stop()
"Get position"
self.rightmargin = self.drive.position
print("Right position : {} ".format(self.rightmargin), file=sys.stderr)
self.drive.on(SpeedPercent(-10))
time.sleep(1)
self.pushsensor.wait_for_pressed ()
self.drive.stop()
"Get position + offset 45 for not to close limitation"
self.leftmargin = self.drive.position
self.leftmargin = self.leftmargin + 60
print("Left position : {} ".format(self.leftmargin), file=sys.stderr)
self.degreeStep = int(abs((self.leftmargin - self.rightmargin)/self.numPlayers))
print("Degree steps : {} ".format(self.degreeStep), file=sys.stderr)
def _addUser (self):
if self.numPlayers == 0:
self.players.clear()
player = self.bcolor.color_name
self.players.append(player.lower())
print("Player {} color: {}".format(self.players[self.numPlayers],player), file=sys.stderr)
self.numPlayers += 1
self._send_event(EventName.PLAYER, {'player': player})
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 seconds
: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), file=sys.stderr)
if direction in Direction.FORWARD.value:
self.drive.on_for_degrees(SpeedPercent(10),90)
if direction in Direction.BACKWARD.value:
self.drive.on_for_degrees(SpeedPercent(-10),90)
if direction in Direction.STOP.value:
self.drive.off()
def _send_event(self, name: EventName, payload):
"""
Sends a custom event to trigger a sentry action.
:param name: the name of the custom event
:param payload: the sentry JSON payload
"""
self.send_custom_event('Custom.Mindstorms.Gadget', name.value, payload)
def _activate(self, command, speed=50):
"""
Handles preset commands.
:param command: the preset command
:param speed: the speed if applicable
"""
print("Activate command: ({}, {})".format(command, speed), file=sys.stderr)
if command in Command.GAMES.value:
self.game = command
print("Play game: {}".format(self.game), file=sys.stderr)
self._gameinit(self.game)
if command in Command.RESET_GAME.value:
# Reset game
self._init_reset()
if command in Command.ADD_CMD.value:
self._addUser()
class KitchenSinkGadget(AlexaGadget):
"""
Class that logs each directive received from the Echo device.
"""
def __init__(self):
super().__init__()
self.leds = Leds()
self.motorOne = LargeMotor(OUTPUT_C)
self.motorTwo = LargeMotor(OUTPUT_B)
self.motorThree = MediumMotor(OUTPUT_A)
#self.t1 = ""
#self.t2 = ""
#self.TouchIt = TouchSensor(INPUT_4)
#ts = TouchSensor()
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','RED')
self.leds.set_color('RIGHT','RED')
threading.Thread(target=self.buttonListener).start()
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')
def on_alexa_gadget_statelistener_stateupdate(self, directive):
"""
Alexa.Gadget.StateListener StateUpdate directive received.
For more info, visit:
https://developer.amazon.com/docs/alexa-gadgets-toolkit/alexa-gadget-statelistener-interface.html#StateUpdate-directive
:param directive: Protocol Buffer Message that was send by Echo device.
To get the specific state update name, the following code snippet can be used:
# Extract first available state (name & value) from directive payload
"""
if len(directive.payload.states) > 0:
state = directive.payload.states[0]
name = state.name
value = state.value
print('state name:{}, state value:{}'.format(name, value))
if name == "timers":
if value == "active":
print("Active hai timer abhi")
self.motorThree.on(20)
#self.motorOne.on_for_degrees(speed = 10, degrees= 90)
#self.motorTwo.on_for_degrees(10,-90)
elif value == "cleared":
print("Timer cleared here now")
self.motorThree.off()
#self.motorThree.on_to_position(10,0)
#for i in range(12,168):
#self.motorOne.on_to_position(2,0)
#self.motorTwo.on_to_position(2,0)
elif name == "wakeword":
actualPos = self.motorOne.position
print(actualPos)
#self.motorOne.on_to_position(10,0)
if value == "active":
self.leds.set_color('LEFT','GREEN')
self.leds.set_color('RIGHT','GREEN')
self.motorOne.on_to_position(10,-10,True,True)
self.motorOne.wait_until_not_moving()
self.motorOne.on_to_position(10,0,True,True)
self.motorOne.on_to_position(10,10,True,True)
self.motorOne.on_to_position(10,0,True,True)
elif value == "cleared":
self.leds.set_color('LEFT','BLACK')
self.leds.set_color('RIGHT','BLACK')
#self.motorOne.on_to_position(20,0)
elif name == "alarms":
if value == "active":
self.leds.set_color('LEFT','RED')
self.leds.set_color('RIGHT','RED')
self.motorThree.on(20)
elif value == "cleared":
self.motorThree.stop()
self.leds.set_color('LEFT','BLACK')
self.leds.set_color('RIGHT','BLACK')
elif name == "reminders":
if value == "active":
self.leds.set_color('LEFT','GREEN')
self.leds.set_color('RIGHT','GREEN')
#self.leds.set_color('UP','GREEN')
#self.leds.set_color('DOWN','GREEN')
moveUp = True
moveDown = True
counter = 0
for i in range(0,15):
#if moveUp:
self.motorTwo.on_to_position(10,-10,True,True)
self.motorThree.on_to_position(5,-20,True,True)
time.sleep(0.3)
self.motorTwo.on_to_position(10,70,True,True)
#moveUp = False
#moveDown = True
#elif moveDown:
# self.motorTwo.on_for_degrees(20,0,True,True)
# moveUp = True
# moveDown = False
elif value == "cleared":
self.leds.set_color('LEFT','BLACK')
self.leds.set_color('RIGHT','BLACK')
self.motorTwo.on_to_position(20,0)
self.motorThree.on_to_position(5,70,True,True)
#self.leds.set_color('UP','BLACK')
#self.leds.set_color('DOWN','BLACK')
def on_notifications_setindicator(self, directive):
"""
Notifications SetIndicator directive received.
For more info, visit:
https://developer.amazon.com/docs/alexa-gadgets-toolkit/notifications-interface.html#SetIndicator-directive
:param directive: Protocol Buffer Message that was send by Echo device.
"""
print("Notification Set Indicator")
def on_notifications_clearindicator(self, directive):
"""
Notifications ClearIndicator directive received.
For more info, visit:
https://developer.amazon.com/docs/alexa-gadgets-toolkit/notifications-interface.html#ClearIndicator-directive
:param directive: Protocol Buffer Message that was send by Echo device.
"""
print("Notification Cleared")
def on_alexa_gadget_musicdata_tempo(self, directive):
"""
Provides the music tempo of the song currently playing on the Echo device.
:param directive: the music data directive containing the beat per minute value
"""
tempo_data = directive.payload.tempoData
for tempo in tempo_data:
print("tempo value: {}".format(tempo.value))
if tempo.value > 0:
# dance pose
print(tempo.value)
#self.right_motor.run_timed(speed_sp=750, time_sp=2500)
#self.left_motor.run_timed(speed_sp=-750, time_sp=2500)
#self.hand_motor.run_timed(speed_sp=750, time_sp=2500)
self.leds.set_color("LEFT", "GREEN")
self.leds.set_color("RIGHT", "GREEN")
time.sleep(3)
# starts the dance loop
self.trigger_bpm = "on"
self.t1 = threading.Thread(target=self._dance_loop, args=(tempo.value,)).start()
self.t2 = threading.Thread(target=self.ledShuffler, args=(tempo.value,)).start()
elif tempo.value == 0:
# stops the dance loop
#print(dir(self.t1))
self.trigger_bpm = "off"
self.motorOne.on_to_position(5,0,True,True)
self.leds.set_color("LEFT", "BLACK")
self.leds.set_color("RIGHT", "BLACK")
def danceMoveTwo(self):
for i in range(0,2):
self.motorThree.on_for_rotations(15,1,True,False)
self.motorTwo.on_to_position(15,-30,True,True)
self.motorOne.on_to_position(5,0,True,True)
self.motorThree.on_for_rotations(15,1,True,False)
self.motorTwo.on_to_position(15,45,True,True)
self.motorOne.on_to_position(5,-60,True,True)
self.motorThree.on_for_rotations(15,1,True,True)
self.motorOne.on_to_position(5,0,True,True)
self.motorThree.on_for_rotations(15,1,True,True)
self.motorOne.on_to_position(5,60,True,True)
self.motorThree.on_for_rotations(15,1,True,True)
self.motorOne.on_to_position(5,0,True,True)
def danceMoveFive(self):
for i in range(0,4):
print("Move five part one")
print(self.motorTwo.position)
self.motorTwo.on_to_position(15,-30,True,False)
#self.motorTwo.wait_until_not_moving()
self.motorTwo.on_to_position(20,40,True,False)
self.motorOne.on_to_position(5,0,True,True)
self.motorOne.on_to_position(5,60,True,False)
#for i in range(0,4):
# print("Move five part two")
# print(self.motorThree.position)
# self.motorThree.on_to_position(15,70,True,True)
# self.motorOne.on_to_position(5,-60,True,True)
# self.motorThree.on_to_position(15,130,True,False)
# self.motorOne.on_to_position(5,-60,True,False)
def danceMoveSix(self):
for i in range(0,12):
if self.trigger_bpm != 'on':
return
moveUp = True
moveDown = True
if moveUp:
print("Move Six Part One")
print(self.motorThree.position)
#self.leds.set_color('LEFT','RED')
#self.leds.set_color('RIGHT','RED')
self.motorThree.on_to_position(10,0,True,True)
self.motorThree.on_to_position(10,70,True,False)
moveUp = False
moveDown = True
if moveDown:
print("Move Six Part Two")
#self.leds.set_color('LEFT','GREEN')
#self.leds.set_color('RIGHT','GREEN')
print(self.motorTwo.position)
self.motorTwo.on_to_position(15,-30,True,True)
self.motorTwo.on_to_position(15,45,True,False)
moveUp = True
moveDown = False
#self.motorTwo.on_to_position(20,-20,True,True)
#self.motorOne.on_to_position(5,60,True,False)
#self.motorTwo.on_to_position(20,20,True,True)
self.motorOne.on_to_position(5,60,True,True)
self.motorOne.on_to_position(1,0,True,True)
def moveSeven(self):
start = 0
for each in range(0,50):
if self.trigger_bpm != 'on':
return
start += 5
self.motorThree.on_to_position(1,start,True,True)
time.sleep(0.2)
def moveHands(self):
self.motorThree.on_to_position(15,30,True,False)
self.motorTwo.on_to_position(20,0,True,True)
self.motorThree.on_to_position(15,-30,True,False)
self.motorTwo.on_to_position(20,55,True,True)
def moveHands2(self):
self.motorThree.on_to_position(10,-120,True,False)
self.motorTwo.on_to_position(15,-10,True,True)
self.motorThree.on_to_position(10,-50,True,False)
self.motorTwo.on_to_position(15,45,True,True)
def danceMoveFour(self):
if self.trigger_bpm != 'on':
return
self.motorOne.on_to_position(8,0,True,True)
for i in range(0,4):
print("In move four part one")
self.moveHands()
self.motorOne.on_to_position(8,-40,True,True)
if self.trigger_bpm != 'on':
return
for i in range(0,4):
print("In move four part two")
self.moveHands()
self.motorOne.on_to_position(8,0,True,True)
if self.trigger_bpm != 'on':
return
for i in range(0,4):
print("In move four part three")
self.moveHands()
self.motorOne.on_to_position(8,40,True,True)
if self.trigger_bpm != 'on':
return
for i in range(0,4):
print("In move four part four")
self.moveHands2()
self.motorOne.on_to_position(8,0,True,True)
if self.trigger_bpm != 'on':
return
for i in range(0,4):
print("In move four part five")
self.moveHands2()
def danceMoveThree(self):
self.motorOne.on_to_position(5,0,True,False)
self.motorTwo.on_to_position(20,-30,True,True)
self.motorTwo.on_to_position(20,45,True,True)
self.motorTwo.on_to_position(20,-30,True,True)
self.motorTwo.on_to_position(20,45,True,True)
self.motorOne.on_to_position(5,-40,True,False)
self.motorTwo.on_to_position(20,-30,True,True)
self.motorTwo.on_to_position(20,45,True,True)
self.motorTwo.on_to_position(20,-30,True,True)
self.motorTwo.on_to_position(20,45,True,True)
self.motorOne.on_to_position(5,0,True,False)
self.motorTwo.on_to_position(20,-30,True,True)
self.motorTwo.on_to_position(20,45,True,True)
self.motorTwo.on_to_position(20,-30,True,True)
self.motorTwo.on_to_position(20,45,True,True)
self.motorOne.on_to_position(5,40,True,False)
self.motorTwo.on_to_position(20,-30,True,True)
self.motorTwo.on_to_position(20,45,True,True)
self.motorTwo.on_to_position(20,-30,True,True)
self.motorTwo.on_to_position(20,45,True,True)
def danceMoveOne(self):
self.motorTwo.on_to_position(20,-30,True,False)
self.motorOne.on_to_position(5,0,True,True)
self.motorTwo.on_to_position(20,45,True,False)
self.motorOne.on_to_position(5,-60,True,True)
self.motorTwo.on_to_position(20,-30,True,False)
self.motorOne.on_to_position(5,0,True,True)
self.motorTwo.on_to_position(20,45,True,False)
self.motorOne.on_to_position(5,60,True,True)
self.motorTwo.on_to_position(20,-30,True,False)
def buttonListener(self):
while True:
if ts.is_pressed:
print("Button Pressed")
self._send_event("buttonPress" ,{"pressed" : "pressedNow"})
else:
pass
time.sleep(0.2)
def ledShuffler(self,bpm):
color_list = ["GREEN", "RED", "AMBER", "YELLOW"]
led_color = random.choice(color_list)
while self.trigger_bpm == "on":
led_color = "BLACK" if led_color != "BLACK" else random.choice(color_list)
self.leds.set_color("LEFT", led_color)
self.leds.set_color("RIGHT", led_color)
milli_per_beat = min(1000, (round(60000 / bpm)) * 0.65)
time.sleep(milli_per_beat / 1000)
def _dance_loop(self, bpm):
"""
Perform motor movement in sync with the beat per minute value from tempo data.
:param bpm: beat per minute from AGT
"""
motor_speed = 400
milli_per_beat = min(1000, (round(60000 / bpm)) * 0.65)
print("Adjusted milli_per_beat: {}".format(milli_per_beat))
while self.trigger_bpm == "on":
# Alternate led color and motor direction
motor_speed = -motor_speed
#self.danceMoveFive()
#self.danceMoveSix()
#self.danceMoveOne()
#self.danceMoveTwo()
#self.danceMoveThree()
if self.trigger_bpm != "on":
break
self.danceMoveFour()
if self.trigger_bpm != "on":
break
self.danceMoveFive()
if self.trigger_bpm != "on":
break
self.danceMoveSix()
if self.trigger_bpm != "on":
break
self.moveSeven()
if self.trigger_bpm != "on":
break
#self.right_motor.run_timed(speed_sp=motor_speed, time_sp=150)
#self.left_motor.run_timed(speed_sp=-motor_speed, time_sp=150)
time.sleep(milli_per_beat / 1000)
print("Exiting BPM process.")
self.motorTwo.on_to_position(5,75,True,True)
self.motorThree.on_to_position(5,75,True,True)
self.motorOne.on_to_position(5,0,True,True)
def _send_event(self, name, payload):
"""
Sends a custom event to trigger a sentry action.
:param name: the name of the custom event
:param payload: the sentry JSON payload
"""
self.send_custom_event('Custom.Mindstorms.Gadget', name, payload)
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
"""
print("Directive", directive)
try:
payload = json.loads(directive.payload.decode("utf-8"))
print("Control payload: {}".format(payload))
control_type = payload["type"]
print("Control Type",control_type)
if control_type == "dance":
print("Move command found")
self.danceMoveFive()
self._send_event("completion", {'danceMove' : 'completed'})
elif control_type == "rotate":
self.motorOne.on_to_position(5,190,True,True)
self.motorOne.on_to_position(5,-150,True,True)
self.motorTwo.on_to_position(10,-30,True,True)
self.motorThree.on_to_position(5,-20,True,True)
time.sleep(1)
#self.motorTwo.on_to_position(5,40,True,True)
#time.sleep(2)
#self.motorOne.on_to_position(5,0,True,True)
self._send_event("completion",{'startGame':'completed'})
elif control_type == "movefinger" :
time.sleep(1.4)
print("Delay 1.4")
self.motorTwo.on_to_position(15,-20,True,True)
self.motorTwo.on_to_position(15,40,True,True)
#while not ts.is_pressed:
# print("Touch Sensor is not pressed")
#self._send_event("completion",{'flying':'made'})
#self._send_event("completion",{'flying':'completed'})
elif control_type == "movefingeragain":
time.sleep(1.2)
print("Delay 1.2")
self.motorTwo.on_to_position(15,-20,True,True)
self.motorTwo.on_to_position(15,40,True,True)
elif control_type == "movefingerfirst":
time.sleep(0.3)
print("Delay 0.3")
self.motorTwo.on_to_position(15,-20,True,True)
self.motorTwo.on_to_position(15,40,True,True)
elif control_type == "chill":
self.motorOne.on_to_position(5,20,True,True)
self.motorTwo.on_to_position(5,55,True,True)
self.motorThree.on_to_position(5,40,True,True)
#self._send_event("completion",{'backtoPos':'completed'})
elif control_type == "rotatetwo":
self.motorOne.on_to_position(5,190,True,True)
self.motorOne.on_to_position(5,-150,True,True)
self.motorOne.on_to_position(5,20,True,True)
time.sleep(0.5)
self.motorTwo.on_to_position(10,-40,True,True)
self.motorThree.on_to_position(5,-30,True,True)
self._send_event("completion",{'playerturn':'completed'})
elif control_type == "startdance":
print("Robot should be dancing now")
time.sleep(4)
self.trigger_bpm = "on"
self.danceMoveFour()
self.danceMoveFive()
self._send_event("completion",{'dance' : 'statue'})
except KeyError:
print("Missing expected parameters: {}".format(directive))
def on_alerts_setalert(self, directive):
"""
Alerts SetAlert directive received.
For more info, visit:
https://developer.amazon.com/docs/alexa-gadgets-toolkit/alerts-interface.html#SetAlert-directive
:param directive: Protocol Buffer Message that was send by Echo device.
"""
print(directive.payload)
#for state in directive.payload.states:
if directive.payload.type == "TIMER":
timeToStart = directive.payload.scheduledTime
token = directive.payload.token
timeNow = datetime.now()
print(timeNow)
#for i in range(12,168):
self.motorTwo.on_to_position(25,120)
self.motorTwo.on_to_position(25,0)
#self.motorTwo.on_to_position(-1,i)
#time.sleep(1.935)
#print(i)
#print("Motor is holding")
#print(self.motorOne.is_holding)
#print("Motor is running")
#print(self.motorOne.is_running)
#print("Motor is stalled")
#print(self.motorOne.is_stalled)
print("Set Alert is done")
elif directive.payload.type == "ALARM":
timeToStart = directive.payload.scheduledTime
token = directive.payload.token
print(timeToStart)
print(token)
self.leds.set_color('LEFT','YELLOW')
self.leds.set_color('RIGHT','YELLOW')
elif directive.payload.type == "REMINDER":
self.leds.set_color('LEFT','AMBER')
self.leds.set_color('RIGHT','AMBER')
#self.leds.set_color('UP','AMBER')
#self.leds.set_color('DOWN','AMBER')
def on_alerts_deletealert(self, directive):
"""
Alerts DeleteAlert directive received.
For more info, visit:
https://developer.amazon.com/docs/alexa-gadgets-toolkit/alerts-interface.html#DeleteAlert-directive
:param directive: Protocol Buffer Message that was send by Echo device.
"""
print(directive.payload)
#for state in directive.payload.states:
# if state.value == "cleared":
# self.motorTwo.run_timed(speed_sp=1000,time_sp=1000)
print("Delete Alert")
#!/usr/bin/env python3
import time
import sys
from ev3dev2.motor import LargeMotor, OUTPUT_A
from ev3dev2.sensor.lego import TouchSensor
from ev3dev2.sensor import INPUT_1
m = LargeMotor(OUTPUT_A)
t = TouchSensor(INPUT_1)
for i in range(10):
print(t.value(), file=sys.stderr)
print(t.value())
time.sleep(1)
m.on_to_position(10, 90)
time.sleep(2)
m.on_to_position(10, 180)
time.sleep(2)
m.on_to_position(10, 90)
time.sleep(2)
print('vse konchilos', file=sys.stderr)
