class Claw:
def __init__(self, motor_address=OUTPUT_D, us_sensor_address=INPUT_2, start_open=True):
self.claw_motor = MediumMotor(motor_address)
self.eyes = UltrasonicSensor(us_sensor_address)
if start_open and not self.is_open():
self.open()
elif not start_open and self.is_open():
self.close()
def open(self):
if self.is_open():
self.close()
self.claw_motor.on_to_position(SpeedPercent(100), 0, brake=False, block=True)
def close(self):
if not self.is_open():
self.open()
self.claw_motor.on_for_rotations(SpeedPercent(100), -1.6 + self.get_rotations(), brake=False, block=True)
def grab_when_within(self, distance_cm=5.0, on_close=None, while_waiting=None, cancel=None):
if not self.is_open():
self.open()
while self.eyes.distance_centimeters >= distance_cm:
if cancel and cancel():
return False
if while_waiting:
while_waiting()
if on_close:
on_close()
self.close()
return True
def release(self):
self.claw_motor.off(brake=False)
def wait_until_distance(self, distance_cm=25, on_wait=None):
while self.eyes.distance_centimeters > distance_cm:
if on_wait:
on_wait()
def get_rotations(self):
return self.claw_motor.rotations
def is_open(self):
return abs(self.get_rotations()) < 0.1
def is_closed(self):
return -1.7 < abs(self.get_rotations()) < -1.5
def main():
'''The main function of our program'''
# set the console just how we want it
reset_console()
set_cursor(OFF)
set_font('Lat15-Terminus24x12')
print('How are you?')
print("")
print("Hello Selina.")
print("Hello Ethan.")
STUD_MM = 8
tank = MoveDifferential(OUTPUT_A, OUTPUT_B, EV3Tire, 16 * STUD_MM)
motorLift = MediumMotor(OUTPUT_D)
sound = Sound()
# sound.speak('How are you master!')
# sound.speak("I like my family")
# sound.speak("I like my sister and i like my brother.")
sound.beep()
eye = InfraredSensor(INPUT_1)
robot = Robot(tank, None, eye)
botton = Button()
while not botton.any():
distance = eye.distance(channel=1)
heading = eye.heading(channel=1)
print('distance: {}, heading: {}'.format(distance, heading))
motorLift.on_to_position(speed=40, position=-7200, block=True) #20 Rounds
if distance is None:
sound.speak("I am lost, there is no beacon!")
else:
if (distance < 14):
tank.off()
sound.speak("I am very close to the beacon!")
motorLift.on_to_position(speed=40, position=7200, block=True)
sound.speak("I had to get some more rubbish.")
sound.speak("Please wait while I lift up my fork.")
tank.turn_right(speed=20, degrees=random.randint(290, 340)) # random.randint(150, 210)
tank.on_for_seconds(left_speed=20, right_speed=20, seconds=20)
tank.turn_right(speed=20, degrees=330)
motorLift.on_to_position(speed=40, position=0, block=True)
elif distance >= 100:
sound.speak("I am too faraway from the beacon")
elif (distance < 99) and (-4 <= heading <= 4): # in right heading
sound.speak("Moving farward")
tank.on(left_speed=20, right_speed=20)
else:
if heading > 0:
tank.turn_left(speed=20, degrees=20)
else:
tank.turn_right(speed=20, degrees=20)
sound.speak("I am finding the beacon.")
time.sleep(0.1)
m1 = DcMotor(OUTPUT_A) # Airjitsu Propeller
m1.duty_cycle_sp = 100
#m2 = DcMotor(OUTPUT_B) # Peristaltic Pump
#m2.duty_cycle_sp=100
m3 = MediumMotor(OUTPUT_C) # Bubble Handle
m3.position = 0
ts1 = TouchSensor(INPUT_1) # Bubble Production
#ts2 = TouchSensor(INPUT_2) # Liquid refill
t = Thread(target=bubble_handle)
bubble_handle_thread = True
t.start()
while True:
if ts1.is_pressed:
running = not running
sleep(0.25)
if running == True:
m1.run_forever()
else:
m1.stop()
# needs to return bubble handle to rest position
sleep(1.7)
m3.wait_until_not_moving()
m3.on_to_position(SpeedPercent(30), 0)
# will never reach this
class LegoBot(MoveDifferential):
def __init__(self, left_motor_port, right_motor_port, rot_motor_port,
wheel_class, wheel_distance_mm,
desc=None, motor_class=LargeMotor):
MoveDifferential.__init__(self, left_motor_port, right_motor_port, wheel_class, wheel_distance_mm)
"""
LegoBot Class inherits all usefull stuff for differential drive
and adds sound, LEDs, IRSensor which is rotated by Medium Motor
"""
self.leds = Leds()
self.sound = Sound()
self.leds.set_color("LEFT", "BLACK")
self.leds.set_color("RIGHT", "BLACK")
# Startup sequence
self.sound.play_song((('C4', 'e'), ('D4', 'e'), ('E5', 'q')))
self.leds.set_color("LEFT", "GREEN")
self.leds.set_color("RIGHT", "GREEN")
# create IR sensors
self.ir_sensor = InfraredSensor()
self.sensor_rotation_point = Pose( 0.05, 0.0, np.radians(0))
self.sensor_rotation_radius = 0.04
self.sensor_thread_run = False
self.sensor_thread_id = None
# temporary storage for ir readings and poses until half rotation is fully made
self.ir_sensors = None
# initialize motion
self.ang_velocity = (0.0,0.0)
self.rot_motor = MediumMotor(rot_motor_port)
self.rotate_thread_run = False
self.rotate_thread_id = None
self.rotation_degrees = 180
# information about robot for controller or supervisor
self.info = Struct()
self.info.wheels = Struct()
self.info.wheels.radius = self.wheel.radius_mm /1000
self.info.wheels.base_length = wheel_distance_mm /1000
self.info.wheels.max_velocity = 2*pi*170/60 # 170 RPM
self.info.wheels.min_velocity = 2*pi*30/60 # 30 RPM
self.info.pose = None
self.info.ir_sensors = Struct()
self.info.ir_sensors.poses = None
self.info.ir_sensors.readings = None
self.info.ir_sensors.rmax = 0.7
self.info.ir_sensors.rmin = 0.04
# starting odometry thread
self.odometry_start(0,0,0)
# start measuring distance with IR Sensor in another thread while rotating
self.sensor_update_start(self.rot_motor)
# start rotating of medium motor
self.rotate_and_update_sensors_start()
def turn_off(self):
# stop odometry thread
self.odometry_stop()
# stop updating sensors
self.rotate_and_update_sensors_stop()
# return robots head to start position
self.rot_motor.on_to_position(100, 0, True, True)
self.sensor_update_stop()
# Shutdown sequence
self.sound.play_song((('E5', 'e'), ('C4', 'e')))
self.leds.set_color("LEFT", "BLACK")
self.leds.set_color("RIGHT", "BLACK")
def get_pose(self):
"""Get the pose of the object in world coordinates"""
return Pose(self.x_pos_mm/1000, self.y_pos_mm/1000, self.theta)
def move(self,dt):
(vl, vr) = self.get_wheel_speeds()
# actual robot move
self.on_for_seconds(SpeedRPS(vl/2/pi), SpeedRPS(vr/2/pi), dt, False, False)
def get_info(self):
# getting updated info for supervisor
self.info.pose = self.get_pose()
return self.info
def set_inputs(self,inputs):
""" Setting new values of (vl, vr) sent by supervisor and controller """
self.set_wheel_speeds(inputs)
def get_wheel_speeds(self):
return self.ang_velocity
def set_wheel_speeds(self,*args):
if len(args) == 2:
(vl, vr) = args
else:
(vl, vr) = args[0]
left_ms = max(-self.info.wheels.max_velocity, min(self.info.wheels.max_velocity, vl))
right_ms = max(-self.info.wheels.max_velocity, min(self.info.wheels.max_velocity, vr))
self.ang_velocity = (left_ms, right_ms)
def sensor_update_start(self, motor, sleep_time=0.005): # 5ms
"""
A thread is started that will run until the user calls sensor_update_stop()
which will set sensor_thread_run to False
"""
self.ir_sensors = {}
def _sensor_monitor():
while self.sensor_thread_run:
angle = -np.radians(motor.degrees) # convert from degrees to radians
sensor_x = round(self.sensor_rotation_radius*cos(angle) + self.sensor_rotation_point.x, 3)
sensor_y = round(self.sensor_rotation_radius*sin(angle) + self.sensor_rotation_point.y, 3)
# adding to temp dict sensor readings and poses
self.ir_sensors.update({(sensor_x, sensor_y, angle):round(self.ir_sensor.proximity*0.007, 3)})
time.sleep(0.005)
self.sensor_thread_id = None
self.sensor_thread_run = True
self.sensor_thread_id = _thread.start_new_thread(_sensor_monitor, ())
def sensor_update_stop(self):
"""
Signal the sensor update thread to exit and wait for it to exit
"""
if self.sensor_thread_id:
self.sensor_thread_run = False
while self.sensor_thread_id:
pass
def rotate_and_update_sensors_start(self):
self.info.ir_sensors.readings = []
self.info.ir_sensors.poses = []
def _rotate_monitor():
while self.rotate_thread_run:
# writing ir sensor reading and poses from temp dict
self.info.ir_sensors.readings = [*self.ir_sensors.values()]
self.info.ir_sensors.poses = [*self.ir_sensors]
# cleaning up temp dict
self.ir_sensors = {}
# rotate rotation motor with sensor
self.rot_motor.on_for_degrees(50, self.rotation_degrees, True, True)
time.sleep(0.005)
# change orientation of rotation
self.rotation_degrees = -self.rotation_degrees
self.rotate_thread_id = None
self.rot_motor.position = 0
# rotate head to the left at start
self.rot_motor.on_for_degrees(100, -90, True, True)
self.rotate_thread_run = True
self.rotate_thread_id = _thread.start_new_thread(_rotate_monitor, ())
def rotate_and_update_sensors_stop(self):
"""
Signal the sensor update thread to exit and wait for it to exit
"""
if self.rotate_thread_id:
self.rotate_thread_run = False
while self.rotate_thread_id:
pass
class SecondStage:
def __init__(self):
self.mvFollowLine = MVFollowLine()
self.steeringDrive = MoveSteering(OUTPUT_B, OUTPUT_C)
self.moveTank = MoveTank(OUTPUT_B, OUTPUT_C)
self.mediumMotor = MediumMotor(OUTPUT_D)
self.mvInfrared = MVInfraredSensor()
def start(self):
# reach line
#self.goUntilDistanceFromWall(27)
self.mvFollowLine.lookForLine()
self.steeringDrive.on_for_seconds(0, SpeedPercent(-40), 0.6)
# turn left
self.moveTank.on_for_rotations(SpeedPercent(40), SpeedPercent(-40), 1.4)
# push button on left
self.goUntilDistanceFromWall(17)
# go back wee bit
self.moveTank.on_for_rotations(SpeedPercent(40), SpeedPercent(40), 2.8)
# turn left
self.moveTank.on_for_rotations(SpeedPercent(40), SpeedPercent(-40), 1.33)
# go to the ramp
#self.goUntilDistanceFromWall(25)
self.mvFollowLine.lookForLine()
self.steeringDrive.on_for_seconds(0, SpeedPercent(-40), 0.4)
# turn left
self.moveTank.on_for_rotations(SpeedPercent(40), SpeedPercent(-40), 1.35)
# go up
self.mvFollowLine.lookForLine()
self.moveTank.on_for_rotations(SpeedPercent(-100), SpeedPercent(-100), 10.5)
timer = time()
while time() - timer < 5:
self.mvFollowLine._followLine(0)
self.moveTank.on_for_rotations(SpeedPercent(20), SpeedPercent(-20), 0.1)
self.moveTank.on_for_rotations(SpeedPercent(-100), SpeedPercent(-100), 6)
self.mvFollowLine.lookForLine()
self.moveTank.on_for_rotations(SpeedPercent(40), SpeedPercent(-40), 2.5)
self.moveTank.on_for_rotations(SpeedPercent(40), SpeedPercent(40), 0.7)
self.mediumMotor.on_to_position(5, -80)
self.moveTank.on_for_rotations(SpeedPercent(-100), SpeedPercent(-100), 0.9)
self.moveTank.on_for_rotations(SpeedPercent(40), SpeedPercent(-40), 0.4)
self.moveTank.on_for_rotations(SpeedPercent(-100), SpeedPercent(-100), 4)
self.mvFollowLine.lookForLine()
self.mediumMotor.on_to_position(5, 0)
timer = time()
while time() - timer < 3:
self.mvFollowLine._followLine(0)
self.moveTank.on_for_rotations(SpeedPercent(-40), SpeedPercent(40), 0.15)
self.moveTank.on_for_rotations(SpeedPercent(-100), SpeedPercent(-100), 11)
self.goUntilDistanceFromWall(40)
self.moveTank.on_for_rotations(SpeedPercent(-40), SpeedPercent(40), 1.3)
self.mvFollowLine.lookForLine()
self.moveTank.on_for_rotations(SpeedPercent(40), SpeedPercent(40), 1.0)
self.moveTank.on_for_rotations(SpeedPercent(-100), SpeedPercent(100), 10)
self.mvFollowLine._followLine(0)
def goUntilDistanceFromWall(self, distance, speed=-40):
while True:
self.steeringDrive.on(0, SpeedPercent(speed))
print(self.mvInfrared.getDistance())
if self.mvInfrared.getDistance() < distance:
self.steeringDrive.off()
break
self.steeringDrive.on_for_seconds(0, SpeedPercent(speed/2), 0.5)
return
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)
#!/usr/bin/env python3 from ev3dev2.motor import MediumMotor, OUTPUT_B from time import sleep mm = MediumMotor() mm.on(speed=50, brake=True, block=False) sleep(1) mm.off() sleep(1) mm.on_for_seconds(speed=50, seconds=2, brake=True, block=True) sleep(1) mm.on_for_rotations(50, 3) sleep(1) mm.on_for_degrees(50, 90) sleep(1) mm.on_to_position(50, 180) sleep(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 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")
class Gripp3r(RemoteControlledTank):
"""
To enable the medium motor toggle the beacon button on the EV3 remote.
"""
CLAW_DEGREES_OPEN = 225
CLAW_DEGREES_CLOSE = 920
CLAW_SPEED_PCT = 50
def __init__(self, left_motor_port=OUTPUT_B, right_motor_port=OUTPUT_C, medium_motor_port=OUTPUT_A):
RemoteControlledTank.__init__(self, left_motor_port, right_motor_port)
self.set_polarity(MediumMotor.POLARITY_NORMAL)
self.medium_motor = MediumMotor(medium_motor_port)
self.ts = TouchSensor()
self.mts = MonitorTouchSensor(self)
self.mrc = MonitorRemoteControl(self)
self.shutdown_event = Event()
# Register our signal_term_handler() to be called if the user sends
# a 'kill' to this process or does a Ctrl-C from the command line
signal.signal(signal.SIGTERM, self.signal_term_handler)
signal.signal(signal.SIGINT, self.signal_int_handler)
self.claw_open(True)
self.remote.on_channel4_top_left = self.claw_close
self.remote.on_channel4_bottom_left = self.claw_open
def shutdown_robot(self):
if self.shutdown_event.is_set():
return
self.shutdown_event.set()
log.info('shutting down')
self.mts.shutdown_event.set()
self.mrc.shutdown_event.set()
self.remote.on_channel4_top_left = None
self.remote.on_channel4_bottom_left = None
self.left_motor.off(brake=False)
self.right_motor.off(brake=False)
self.medium_motor.off(brake=False)
self.mts.join()
self.mrc.join()
def signal_term_handler(self, signal, frame):
log.info('Caught SIGTERM')
self.shutdown_robot()
def signal_int_handler(self, signal, frame):
log.info('Caught SIGINT')
self.shutdown_robot()
def claw_open(self, state):
if state:
# Clear monitor_ts while we are opening the claw. We do this because
# the act of opening the claw presses the TouchSensor so we must
# tell mts to ignore that press.
self.mts.monitor_ts.clear()
self.medium_motor.on(speed=self.CLAW_SPEED_PCT * -1, block=True)
self.medium_motor.off()
self.medium_motor.reset()
self.medium_motor.on_to_position(speed=self.CLAW_SPEED_PCT,
position=self.CLAW_DEGREES_OPEN,
brake=False, block=True)
self.mts.monitor_ts.set()
def claw_close(self, state):
if state:
self.medium_motor.on_to_position(speed=self.CLAW_SPEED_PCT,
position=self.CLAW_DEGREES_CLOSE)
def main(self):
self.mts.start()
self.mrc.start()
self.shutdown_event.wait()
from ev3dev2.motor import MediumMotor, OUTPUT_A
from ev3dev2.sound import Sound
from ev3dev2.button import Button
import time
medMot = MediumMotor(OUTPUT_A)
medMot.stop_action = 'hold'
mySnd = Sound()
medMot.on_to_position(30, 0)
mySnd.speak("looking left and right")
medMot.on_to_position(10, 90)
time.sleep(0.5)
medMot.on_to_position(10, -90)
time.sleep(0.5)
medMot.on_to_position(10, 0)
mySnd.speak("spinning")
medMot.on_for_seconds(80, 1)
medMot.on_to_position(50, 0)
mySnd.speak("fixed turn")
for i in range(12):
medMot.on_for_degrees(30, 30)
time.sleep(1.0)
medMot.stop()
bttn = Button()
mySnd.speak("Turns until button pressed")
