class Gripp3r(IRBeaconRemoteControlledTank):
def __init__(self,
left_motor_port: str = OUTPUT_B,
right_motor_port: str = OUTPUT_C,
grip_motor_port: str = OUTPUT_A,
touch_sensor_port: str = INPUT_1,
ir_sensor_port: str = INPUT_4,
ir_beacon_channel: int = 1):
super().__init__(left_motor_port=left_motor_port,
right_motor_port=right_motor_port,
ir_sensor_port=ir_sensor_port,
ir_beacon_channel=ir_beacon_channel)
self.grip_motor = MediumMotor(address=grip_motor_port)
self.touch_sensor = TouchSensor(address=touch_sensor_port)
self.ir_sensor = InfraredSensor(address=ir_sensor_port)
self.ir_beacon_channel = ir_beacon_channel
self.speaker = Sound()
def grip_or_release_by_ir_beacon(self, speed: float = 50):
if self.ir_sensor.beacon(channel=self.ir_beacon_channel):
if self.touch_sensor.is_pressed:
self.speaker.play_file(
wav_file='/home/robot/sound/Air release.wav',
volume=100,
play_type=Sound.PLAY_NO_WAIT_FOR_COMPLETE)
self.grip_motor.on_for_seconds(speed=speed,
seconds=1,
brake=True,
block=True)
else:
self.speaker.play_file(
wav_file='/home/robot/sound/Airbrake.wav',
volume=100,
play_type=Sound.PLAY_NO_WAIT_FOR_COMPLETE)
self.grip_motor.on(speed=-speed, brake=False, block=False)
self.touch_sensor.wait_for_pressed()
self.grip_motor.off(brake=True)
while self.ir_sensor.beacon(channel=self.ir_beacon_channel):
pass
def main(self, speed: float = 100):
self.grip_motor.on_for_seconds(speed=-50,
seconds=1,
brake=True,
block=True)
while True:
self.drive_once_by_ir_beacon(speed=speed)
self.grip_or_release_by_ir_beacon()
class CuriosityRov3r(IRBeaconRemoteControlledTank):
def __init__(
self,
left_motor_port: str = OUTPUT_B, right_motor_port: str = OUTPUT_C,
medium_motor_port: str = OUTPUT_A,
touch_sensor_port: str = INPUT_1, color_sensor_port: str = INPUT_3,
ir_sensor_port: str = INPUT_4, ir_beacon_channel: int = 1):
super().__init__(
left_motor_port=left_motor_port, right_motor_port=right_motor_port,
ir_sensor_port=ir_sensor_port, ir_beacon_channel=ir_beacon_channel)
self.medium_motor = MediumMotor(address=medium_motor_port)
self.touch_sensor = TouchSensor(address=touch_sensor_port)
self.color_sensor = ColorSensor(address=color_sensor_port)
self.ir_sensor = InfraredSensor(address=ir_sensor_port)
self.ir_beacon_channel = ir_beacon_channel
self.dis = Display()
self.noise = Sound()
def spin_fan(self, speed: float = 100):
if self.color_sensor.reflected_light_intensity > 20:
self.medium_motor.on(
speed=speed,
brake=False,
block=False)
else:
self.medium_motor.off(brake=True)
def say_when_touched(self):
if self.touch_sensor.is_pressed:
self.dis.image_filename(
filename='/home/robot/image/Angry.bmp',
clear_screen=True)
self.dis.update()
self.noise.play_file(
wav_file='/home/robot/sound/No.wav',
volume=100,
play_type=Sound.PLAY_WAIT_FOR_COMPLETE)
self.medium_motor.on_for_seconds(
speed=-50,
seconds=3,
brake=True,
block=True)
def main(self, speed: float = 100):
while True:
self.drive_once_by_ir_beacon(speed=speed)
self.say_when_touched()
self.spin_fan(speed=speed)
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
class Claw(object):
def __init__(self):
self.closed = False
self.motor = MediumMotor(OUTPUT_A)
def releaseClaw(self):
self.motor.on(-50)
self.motor.wait_until_not_moving(timeout=500)
self.motor.off()
self.closed = False
def closeClaw(self):
self.motor.on(50)
self.motor.wait_until_not_moving(timeout=500)
self.motor.off()
self.closed = True
class FlySwatter:
def __init__(self, sensorMotorOutput=OUTPUT_C, touchSensorInput=INPUT_4):
self._sensorMotor = MediumMotor(sensorMotorOutput)
self._touchSensor = UltrasonicSensor(touchSensorInput)
self._sensorReadings = []
def scan_right(self):
self._sensorReadings = []
num_rotations = self._sensorMotor.rotations
self._sensorMotor.on_for_rotations(SPEED, rotations=0.25, block=False)
while self._sensorMotor.rotations - num_rotations <= 0.25:
self._sensorReadings.append(
(round(self._sensorMotor.rotations,
2), round(self._touchSensor.distance_centimeters, 2)))
print(self._sensorReadings)
self._sensorMotor.off(brake=False)
def scan_left(self):
self._sensorReadings = []
num_rotations = self._sensorMotor.rotations
self._sensorMotor.on_for_rotations(SPEED, rotations=-0.25, block=False)
while self._sensorMotor.rotations - num_rotations > -0.25:
print(self._sensorMotor.rotations - num_rotations)
self._sensorReadings.append(
(round(self._sensorMotor.rotations,
2), round(self._touchSensor.distance_centimeters, 2)))
print(self._sensorReadings)
self._sensorMotor.off(brake=False)
def take_in_readings_from_sensor(self):
self._sensorReadings = []
for x in range(0, 150):
self._sensorReadings.append(
(self._sensorMotor.rotations,
round(self._touchSensor.distance_centimeters, 2)))
return self._sensorReadings
def get_sensor_readings(self):
tmp_readings = []
for pair in self._sensorReadings:
tmp_readings.append(pair[1])
return tmp_readings
class Claw:
def __init__(self, motor_address=OUTPUT_D, us_sensor_address=INPUT_2):
self._claw = MediumMotor(motor_address)
self._us_sensor = UltrasonicSensor(us_sensor_address)
self.is_open = False
def open(self):
if self.is_open:
self.close()
self._claw.on_for_degrees(SpeedPercent(50),
570,
brake=False,
block=True)
self.is_open = True
def close(self):
if not self.is_open:
self.open()
self._claw.on_for_degrees(SpeedPercent(50),
-570,
brake=False,
block=True)
self.is_open = False
def grab_when_within(self,
distance_cm=5.0,
while_waiting=None,
cancel=None):
self.open()
while self._us_sensor.distance_centimeters >= distance_cm:
if cancel and cancel():
return False
if while_waiting:
while_waiting()
self.close()
return True
def release(self):
self._claw.off()
async def on_connect(socket, path):
movesteering = MoveSteering(OUTPUT_A, OUTPUT_B)
fork = MediumMotor(OUTPUT_C)
color_sensor = ColorSensor(address=INPUT_1)
while True:
try:
raw_cmd = await asyncio.wait_for(socket.recv(), timeout=500)
if raw_cmd != "":
command = json.loads(raw_cmd)
command_type = command['type']
print("MOVE COMMAND")
print(command)
if command_type == 'MOVE':
move = command['move']
if move == 'w':
movesteering.on(0, 100)
elif move == 's':
movesteering.on(0, -100)
elif move == 'a':
movesteering.on(100, 100)
elif move == 'd':
movesteering.on(-100, 100)
elif move == 't':
fork.on(-100)
elif move == 'g':
fork.on(100)
elif move == 'c':
print(color_sensor.color_name)
elif move == 'stop':
movesteering.off()
fork.off()
except TimeoutError:
pass
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 Rov3r(IRBeaconRemoteControlledTank):
def __init__(
self,
left_motor_port: str = OUTPUT_B, right_motor_port: str = OUTPUT_C,
gear_motor_port: str = OUTPUT_A,
touch_sensor_port: str = INPUT_1, color_sensor_port: str = INPUT_3,
ir_sensor_port: str = INPUT_4, ir_beacon_channel: int = 1):
super().__init__(
left_motor_port=left_motor_port, right_motor_port=right_motor_port,
ir_sensor_port=ir_sensor_port, ir_beacon_channel=ir_beacon_channel)
self.gear_motor = MediumMotor(address=gear_motor_port)
self.touch_sensor = TouchSensor(address=touch_sensor_port)
self.color_sensor = ColorSensor(address=color_sensor_port)
self.ir_sensor = InfraredSensor(address=ir_sensor_port)
self.ir_beacon_channel = ir_beacon_channel
self.speaker = Sound()
self.dis = Display(desc='Display')
def spin_gears(self, speed: float = 100):
while True:
if self.ir_sensor.beacon(channel=self.ir_beacon_channel):
self.gear_motor.on(
speed=speed,
block=False,
brake=False)
else:
self.gear_motor.off(brake=True)
def change_screen_when_touched(self):
while True:
if self.touch_sensor.is_pressed:
self.dis.image_filename(
filename='/home/robot/image/Angry.bmp',
clear_screen=True)
self.dis.update()
else:
self.dis.image_filename(
filename='/home/robot/image/Fire.bmp',
clear_screen=True)
self.dis.update()
def make_noise_when_seeing_black(self):
while True:
if self.color_sensor.color == ColorSensor.COLOR_BLACK:
self.speaker.play_file(
wav_file='/home/robot/sound/Ouch.wav',
volume=100,
play_type=Sound.PLAY_WAIT_FOR_COMPLETE)
def main(self):
self.speaker.play_file(
wav_file='/home/robot/sound/Yes.wav',
volume=100,
play_type=Sound.PLAY_WAIT_FOR_COMPLETE)
Thread(target=self.make_noise_when_seeing_black,
daemon=True).start()
Thread(target=self.spin_gears,
daemon=True).start()
Thread(target=self.change_screen_when_touched,
daemon=True).start()
self.keep_driving_by_ir_beacon(speed=100)
"EV3_Service",
service_id=uuid,
service_classes=[uuid, bluetooth.SERIAL_PORT_CLASS],
profiles=[bluetooth.SERIAL_PORT_PROFILE])
print("Waiting for connection of RFCOMM channel %d" % port)
speaker.speak("Waiting for connection of RFCOMM channel")
client_socket, address = server_socket.accept()
print("Accepted connection from", address)
speaker.speak("Accepted connection from external device")
while True:
data = client_socket.recv(1024)
if len(data) == 0:
break
data = roboarm_format_name(data)
print(data)
if data == "up":
mMotor.on(speed=50)
elif data == "down":
mMotor.on(speed=-50)
elif data == "idle":
mMotor.off()
#speaker.speak(data)
if data == "esc":
break
client_socket.close()
server_socket.close()
class MindstormsGadget(AlexaGadget):
"""
A Mindstorms gadget that can perform bi-directional interaction with an Alexa skill.
"""
def __init__(self):
"""
Performs Alexa Gadget initialization routines and ev3dev resource allocation.
"""
super().__init__()
# the default I2C address of the sensor
self.I2C_ADDRESS = 0x21
# setup the buses
self.airqualitybus = SMBus(3)
self.temperaturebus = SMBus(4)
#setup the moisbus and relaybus
self.airqualitybus.write_byte_data(self.I2C_ADDRESS, 0x42, 0x01)
self.temperaturebus.write_byte_data(self.I2C_ADDRESS, 0x42, 0x01)
#setup the lastmois so we can track it well
self.lastairquality = 0
self.lasttemperature = 0
self.count = 0
self.speed = 0
# Robot state
self.auto_mode = False
self.filterwarning = False
self.sound = Sound()
self.leds = Leds()
self.color = ColorSensor()
self.touch = TouchSensor()
#Motor
self.fan = MediumMotor(OUTPUT_A)
#screen
self.screen = ev3.Screen()
# Start threads
threading.Thread(target=self._autofan_thread, daemon=True).start()
threading.Thread(target=self._manual_button_thread,
daemon=True).start()
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 == "airquality":
self._airquality_handler()
elif control_type == "temperature":
self._temperature_handler(payload["unit"])
elif control_type == "speed":
self._speed_handler(payload["speed"])
elif control_type == "auto":
self._auto_handler(payload["command"])
except KeyError:
print("Missing expected parameters: {}".format(directive),
file=sys.stderr)
def _speed_handler(self, speed):
print(speed)
self.speed = speed
if speed != 0:
self.fan.on(SpeedPercent(-speed))
else:
self.fan.on(SpeedPercent(speed))
self.fan.off()
def _airquality_handler(self):
print(self.lastairquality)
if self.lastairquality > 700:
if self.auto_mode == True:
self._send_event(
EventName.AIRQUALITY, {
'request':
0,
'speech':
"We are currently experiencing high pollution, air filter is set to high automatically"
})
else:
self._send_event(
EventName.AIRQUALITY, {
'request':
1,
'speech':
"We are currently experiencing high pollution, would you like to set the air purifier to high mode?"
})
elif self.lastairquality > 300:
if self.auto_mode == True:
self._send_event(
EventName.AIRQUALITY, {
'request':
0,
'speech':
"We are currently experiencing low pollution, air filter is set to high automatically"
})
else:
self._send_event(
EventName.AIRQUALITY, {
'request':
1,
'speech':
"We are currently experiencing low pollution, would you like to set the air purifier to high mode?"
})
else:
self._send_event(EventName.AIRQUALITY, {
'request': 0,
'speech': "The air quality is fresh and clean."
})
def _temperature_handler(self, unit):
print(self.lasttemperature)
if unit.lower() == 'fahrenheit':
fahrenheit = self.lasttemperature * 9 / 5 + 32
print(fahrenheit)
self._send_event(
EventName.TEMPERATURE, {
'speech':
"The temperature in the room is " + str(int(fahrenheit)) +
" degrees fahrenheit"
})
else:
self._send_event(
EventName.TEMPERATURE, {
'speech':
"The temperature in the room is " +
str(int(self.lasttemperature)) + " degrees celcius"
})
def _auto_handler(self, onoff):
if onoff == "on":
self.auto_mode = True
else:
self.auto_mode = False
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
"""
print(name.value)
print(payload)
self.send_custom_event('Custom.Mindstorms.Gadget', name.value, payload)
def _autofan_thread(self):
"""
Performs random movement when patrol mode is activated.
"""
print('fan thread started')
while True:
self.screen.clear()
#Air Quality
self.screen.draw.rectangle((0, 0, 177, 40), fill='black')
part1 = self.airqualitybus.read_byte_data(self.I2C_ADDRESS, 0x44)
part2 = self.airqualitybus.read_byte_data(self.I2C_ADDRESS, 0x45)
aq = (part1 << 2) + part2
print("Air Quality:" + str(aq))
self.screen.draw.text((36, 13),
"Air Quality:" + str(aq),
fill='white')
#temperature
part3 = self.temperaturebus.read_byte_data(self.I2C_ADDRESS, 0x44)
part4 = self.temperaturebus.read_byte_data(self.I2C_ADDRESS, 0x45)
temp = (part3 << 2) + part4
R = 1023.0 / temp - 1.0
R = R0 * R
temperature = 1.0 / (math.log(R / R0) / B + 1 / 298.15) - 273.15
print("Temperature:" + str(temperature))
self.screen.draw.text(
(36, 60), "Temperature:" + str(int(temperature)) + " C")
print("color:" + self.color.color_name)
self.screen.draw.text((36, 90), "Filter:" + self.color.color_name)
self.screen.update()
if self.auto_mode and aq >= 300 and self.lastairquality < 300:
#got dirty
self._speed_handler(100)
self._send_event(EventName.FANSPEED, {
'speech':
'Pollution detected, auto setting fan speed to high'
})
elif self.auto_mode and aq <= 300 and self.lastairquality > 300:
#got clean
self._speed_handler(25)
self._send_event(EventName.FANSPEED, {
'speech':
'Air is clean now, auto setting fan speed to low'
})
if aq >= 700:
gadget.leds.set_color("LEFT", "RED")
elif aq >= 300:
gadget.leds.set_color("LEFT", "YELLOW")
else:
gadget.leds.set_color("LEFT", "GREEN")
self.lastairquality = aq
self.lasttemperature = temperature
#Filter maintenance
if self.color.color == ColorSensor.COLOR_WHITE or self.color.color == ColorSensor.COLOR_YELLOW:
self.filterwarning = False
gadget.leds.set_color("RIGHT", "GREEN")
else:
#reset
gadget.leds.set_color("RIGHT", "YELLOW")
if self.filterwarning == False:
self._send_event(
EventName.FILTER, {
'speech':
'The filter seems dirty, please check it and see if it needs to be replaced'
})
self.filterwarning = True
time.sleep(1)
def _manual_button_thread(self):
pressed = False
while True:
if self.touch.is_pressed == True:
pressed = True
if pressed == True and self.touch.is_released == True:
#confirming pressed the button once
pressed = False
if self.speed == 0:
#it's currently off
self._speed_handler(25)
self._send_event(
EventName.FANSPEED,
{'speech': 'Air purifier is setted to low manually'})
elif self.speed < 60:
self._speed_handler(60)
self._send_event(EventName.FANSPEED, {
'speech':
'Air purifier is setted to medium manually'
})
elif self.speed < 100:
self._speed_handler(100)
self._send_event(
EventName.FANSPEED,
{'speech': 'Air purifier is setted to high manually'})
else:
self._speed_handler(0)
self._send_event(
EventName.FANSPEED,
{'speech': 'Air purifier is turned off manually'})
time.sleep(0.1)
class R3ptar:
def __init__(self,
turn_motor_port: str = OUTPUT_A,
move_motor_port: str = OUTPUT_B,
scare_motor_port: str = OUTPUT_D,
touch_sensor_port: str = INPUT_1,
color_sensor_port: str = INPUT_3,
ir_sensor_port: str = INPUT_4,
ir_beacon_channel: int = 1):
self.turn_motor = MediumMotor(address=turn_motor_port)
self.move_motor = LargeMotor(address=move_motor_port)
self.scare_motor = LargeMotor(address=scare_motor_port)
self.touch_sensor = TouchSensor(address=touch_sensor_port)
self.color_sensor = ColorSensor(address=color_sensor_port)
self.ir_sensor = InfraredSensor(address=ir_sensor_port)
self.ir_beacon_channel = ir_beacon_channel
self.noise = Sound()
def keep_driving_by_ir_beacon(self, speed: float = 100):
while True:
if self.ir_sensor.top_left(channel=self.ir_beacon_channel) and \
self.ir_sensor.top_right(channel=self.ir_beacon_channel):
self.move_motor.on(speed=speed, brake=False, block=False)
elif self.ir_sensor.bottom_left(channel=self.ir_beacon_channel) and \
self.ir_sensor.bottom_right(channel=self.ir_beacon_channel):
self.move_motor.on(speed=-speed, brake=False, block=False)
elif self.ir_sensor.top_left(channel=self.ir_beacon_channel):
self.turn_motor.on(speed=-50, brake=False, block=False)
self.move_motor.on(speed=speed, brake=False, block=False)
elif self.ir_sensor.top_right(channel=self.ir_beacon_channel):
self.turn_motor.on(speed=50, brake=False, block=False)
self.move_motor.on(speed=speed, brake=False, block=False)
elif self.ir_sensor.bottom_left(channel=self.ir_beacon_channel):
self.turn_motor.on(speed=-50, brake=False, block=False)
self.move_motor.on(speed=-speed, brake=False, block=False)
elif self.ir_sensor.bottom_right(channel=self.ir_beacon_channel):
self.turn_motor.on(speed=50, brake=False, block=False)
self.move_motor.on(speed=-speed, brake=False, block=False)
else:
self.turn_motor.off(brake=True)
self.move_motor.off(brake=False)
def bite_by_ir_beacon(self, speed: float = 100):
while True:
if self.ir_sensor.beacon(channel=self.ir_beacon_channel):
self.scare_motor.on_for_seconds(speed=speed,
seconds=1,
brake=True,
block=False)
self.noise.play_file(
wav_file='/home/robot/sound/Snake hiss.wav',
volume=100,
play_type=Sound.PLAY_NO_WAIT_FOR_COMPLETE)
self.scare_motor.on_for_seconds(speed=-speed,
seconds=1,
brake=True,
block=True)
while self.ir_sensor.beacon(channel=self.ir_beacon_channel):
pass
def run_away_if_chased(self):
while True:
if self.color_sensor.reflected_light_intensity > 30:
self.move_motor.on_for_seconds(speed=50,
seconds=4,
brake=True,
block=False)
for i in range(2):
self.turn_motor.on_for_seconds(speed=50,
seconds=1,
brake=False,
block=True)
self.turn_motor.on_for_seconds(speed=-50,
seconds=1,
brake=False,
block=True)
def bite_if_touched(self):
while True:
if self.touch_sensor.is_pressed:
self.scare_motor.on_for_seconds(speed=100,
seconds=1,
brake=True,
block=False)
self.noise.play_file(
wav_file='/home/robot/sound/Snake hiss.wav',
volume=100,
play_type=Sound.PLAY_NO_WAIT_FOR_COMPLETE)
self.scare_motor.on_for_seconds(speed=-10,
seconds=10,
brake=True,
block=True)
def main(self, speed: float = 100):
Thread(target=self.bite_by_ir_beacon).start()
Thread(target=self.bite_if_touched).start()
Thread(target=self.run_away_if_chased).start()
self.keep_driving_by_ir_beacon(speed=speed)
#!/usr/bin/env python3 from ev3dev2.motor import MediumMotor, OUTPUT_A from ev3dev2.motor import SpeedDPS, SpeedRPS, SpeedRPM from time import sleep medium_motor = MediumMotor(OUTPUT_A) # We'll make the motor turn 180 degrees # at speed 50 (780 dps for the medium motor) medium_motor.on_for_degrees(speed=50, degrees=180) # then wait one second sleep(1) # then – 180 degrees at 500 dps medium_motor.on_for_degrees(speed=SpeedDPS(500), degrees=-180) sleep(1) # then 0.5 rotations at speed 50 (780 dps) medium_motor.on_for_rotations(speed=50, rotations=0.5) sleep(1) # then – 0.5 rotations at 1 rotation per second (360 dps) medium_motor.on_for_rotations(speed=SpeedRPS(1), rotations=-0.5) sleep(1) medium_motor.on_for_seconds(speed=SpeedRPM(60), seconds=5) sleep(1) medium_motor.on(speed=60) sleep(2) medium_motor.off()
class Ev3Robot:
def __init__(self,
wheel1=OUTPUT_B,
wheel2=OUTPUT_C,
wheel3=OUTPUT_A,
wheel4=OUTPUT_D):
self.steer_pair = MoveSteering(wheel1, wheel2)
self.gyro = GyroSensor()
self.tank_pair = MoveTank(wheel1, wheel2)
self.motor1 = LargeMotor(wheel1)
self.motor2 = LargeMotor(wheel2)
self.motor3 = MediumMotor(wheel3)
self.motor4 = MediumMotor(wheel4)
self.color1 = ColorSensor(INPUT_1)
self.color4 = ColorSensor(INPUT_4)
self._black1 = 0
self._black4 = 0
self._white1 = 100
self._white4 = 100
self.gyro.mode = 'GYRO-ANG'
self.led = Leds()
self.btn = Button()
self.btn._state = set()
def write_color(self, file, value):
# opens a file
f = open(file, "w")
# writes a value to the file
f.write(str(value))
f.close()
def read_color(self, file):
# opens a file
f = open(file, "r")
# reads the value
color = int(f.readline().strip())
f.close()
return color
def pivot_right(self, degrees, speed=20):
# makes the robot pivot to the right until the gyro sensor
# senses that it has turned the required number of degrees
self.tank_pair.on(left_speed=speed, right_speed=0)
self.gyro.wait_until_angle_changed_by(degrees - 10)
self.tank_pair.off()
def pivot_left(self, degrees, speed=20):
# makes the robot pivot to the left until the gyro sensor
# senses that it has turned the required number of degrees
self.tank_pair.on(left_speed=0, right_speed=speed)
self.gyro.wait_until_angle_changed_by(degrees - 10)
self.tank_pair.off()
def old_spin_right(self, degrees, speed=20):
#old program; don't use
self.gyro.reset()
value1 = self.gyro.angle
self.tank_pair.on(left_speed=speed, right_speed=speed * -1)
self.gyro.wait_until_angle_changed_by(degrees)
value2 = self.gyro.angle
self.tank_pair.on(left_speed=-30, right_speed=30)
self.gyro.wait_until_angle_changed_by(value1 - value2 - degrees)
self.stop()
def old_spin_left(self, degrees, speed=20):
#old program; don't use
value1 = self.gyro.angle
self.tank_pair.on(left_speed=speed * -1, right_speed=speed)
self.gyro.wait_until_angle_changed_by(degrees)
value2 = self.gyro.angle
self.tank_pair.on(left_speed=8, right_speed=-8)
self.gyro.wait_until_angle_changed_by(value2 - value1 - degrees + 5)
self.tank_pair.off()
def dog_gear(self, degrees, motor, speed=30):
if motor == 3:
self.motor3.on_for_degrees(degrees=degrees, speed=speed)
self.motor3.off()
else:
self.motor4.on_for_degrees(degrees=degrees, speed=speed)
def go_straight_forward(self, cm, speed=20, wiggle_factor=1):
value1 = self.motor1.position
angle0 = self.gyro.angle
rotations = cm / 19.05 #divides by circumference of the wheel
# calculates the amount of degrees the robot has turned, then turns the
# opposite direction and repeats until the robot has gone the required
# number of centimeters
while abs(self.motor1.position - value1) / 360 < rotations:
angle = self.gyro.angle - angle0
self.steer_pair.on(steering=angle * wiggle_factor * -1,
speed=speed * -1)
self.steer_pair.off()
def go_straight_backward(self, cm, speed=20, wiggle_factor=1):
# see go_straight_forward
value1 = self.motor1.position
angle0 = self.gyro.angle
rotations = cm / 19.05
while abs(self.motor1.position - value1) / 360 < rotations:
angle = self.gyro.angle - angle0
self.steer_pair.on(steering=angle * wiggle_factor, speed=speed)
self.steer_pair.off()
def calibrate(self):
# turns the led lights orange, and waits for a button to be pressed
# (signal that the robot is on black), then calculates the reflected
# light intensity of the black line, then does the same on the white line
self.led.set_color('LEFT', 'ORANGE')
self.led.set_color('RIGHT', 'ORANGE')
while not self.btn.any():
sleep(0.01)
self.led.set_color('LEFT', 'GREEN')
self.led.set_color('RIGHT', 'GREEN')
self._black1 = self.color1.reflected_light_intensity
self._black4 = self.color4.reflected_light_intensity
sleep(2)
self.led.set_color('LEFT', 'ORANGE')
self.led.set_color('RIGHT', 'ORANGE')
while not self.btn.any():
sleep(0.01)
self.led.set_color('LEFT', 'GREEN')
self.led.set_color('RIGHT', 'GREEN')
self._white1 = self.color1.reflected_light_intensity
self._white4 = self.color4.reflected_light_intensity
sleep(3)
self.write_color("/tmp/black1", self._black1)
self.write_color("/tmp/black4", self._black4)
self.write_color("/tmp/white1", self._white1)
self.write_color("/tmp/white4", self._white4)
def read_calibration(self):
# reads the color files
self._black1 = self.read_color("/tmp/black1")
self._black4 = self.read_color("/tmp/black4")
self._white1 = self.read_color("/tmp/white1")
self._white4 = self.read_color("/tmp/white4")
def align_white(self, speed=20, t=96.8, direction=1):
# goes forward until one of the color sensors sees the white line.
while self.calibrate_RLI(self.color1) < t and self.calibrate_RLI(
self.color4) < t:
self.steer_pair.on(steering=0, speed=speed * direction)
self.steer_pair.off()
# determines which sensor sensed the white line, then moves the opposite
# motor until both sensors have sensed the white line
if self.calibrate_RLI(self.color4) > t:
while self.calibrate_RLI(self.color1) < t:
self.motor1.on(speed=speed * direction)
self.motor1.off()
else:
while self.calibrate_RLI(self.color4) < t:
self.motor2.on(speed=speed * direction)
self.motor2.off()
def align_black(self, speed=20, t=4.7, direction=1):
# see align_white
while self.calibrate_RLI(self.color1) > t and self.calibrate_RLI(
self.color4) > t:
self.steer_pair.on(steering=0, speed=speed * direction)
self.steer_pair.off()
if self.calibrate_RLI(self.color4) < t:
while self.calibrate_RLI(self.color1) > t:
self.motor1.on(speed=speed * direction)
self.motor1.off()
else:
while self.calibrate_RLI(self.color4) > t:
self.motor2.on(speed=speed * direction)
self.motor2.off()
def align(self, t, speed=-20, direction=1):
# aligns three times for extra accuracy
self.align_white(speed=speed, t=100 - t, direction=direction)
self.align_black(speed=-5, t=t, direction=direction)
self.align_white(speed=-5, t=100 - t, direction=direction)
def calibrate_RLI(self, color_sensor):
# returns a scaled value based on what black and white are
if (color_sensor.address == INPUT_1):
black = self._black1
white = self._white1
else:
black = self._black4
white = self._white4
return (color_sensor.reflected_light_intensity - black) / (white -
black) * 100
def stop(self):
self.tank_pair.off()
def spin_right(self, degrees, speed=30):
self.turn(degrees, 100, speed)
def spin_left(self, degrees, speed=30):
self.turn(degrees, -100, speed)
def turn(self, degrees, steering, speed=30):
# turns at a decreasing speed until it turns the required amount of degrees
value1 = self.gyro.angle
s1 = speed
d1 = 0
while d1 < degrees - 2:
value2 = self.gyro.angle
d1 = abs(value1 - value2)
slope = speed / degrees
s1 = (speed - slope * d1) * (degrees / 90) + 3
self.steer_pair.on(steering=steering, speed=s1)
self.steer_pair.off()
(392, 25, 100), (784, 350, 100), (739.98, 250, 100), (698.46, 25, 100),
(659.26, 25, 100), (622.26, 25, 100), (659.26, 50, 400), (415.3, 25, 200),
(554.36, 350, 100), (523.25, 250, 100), (493.88, 25, 100),
(466.16, 25, 100), (440, 25, 100), (466.16, 50, 400), (311.13, 25, 200),
(392, 350, 100), (311.13, 250, 100), (466.16, 25, 100),
(392.00, 300, 150), (311.13, 250, 100), (466.16, 25, 100), (392, 700)
], play_type=Sound.PLAY_NO_WAIT_FOR_COMPLETE),
1: lambda: (sound.speak("Chomp") for _ in range(5))
}
switch.get(0)
while button_pressed.value == 0:
switch.get(child_conn.recv())()
values = [Value('i', 0), Pipe()]
procs = start_thread(target=[grab_everything, play_sonic, run_big_claw], value=values)
while not button.any():
print('Proximity(cm): {}'.format(proximity.distance_centimeters))
print('Claw(\u00B0): {}'.format(claw.degrees))
values[0].value = 1
procs[0].join()
procs[1].join()
procs[2].join()
wheels.off()
claw.off()
initial_rotations = proximity.rot
with clientsocket:
try:
while True:
c = receive_from_socket(clientsocket)
if not c:
break
command, *params = c.split(" ")
if command == "MOVE":
move_forward(float(params[0]))
elif command == "ROTATE":
rotate(float(params[0]))
elif command == "SCAN":
precision = float(params[0])
num_scans = float(params[1])
increasing = params[2] == "True"
scan(precision, num_scans, increasing)
elif command == "ROTATESENSOR":
rotate_sensor(float(params[0]), speed=20)
else:
print("Unknown command: " + command)
except (KeyboardInterrupt, RuntimeError, OSError):
pass
except Exception as e:
print(e)
say("Shut down")
rotate_sensor_to_zero_position()
steer_pair.off()
motor_sensor.off()
class SuperTurtle:
def __init__(self,
left_leg_motor_port: str = OUTPUT_B,
right_leg_motor_port: str = OUTPUT_C,
shooting_motor_port: str = OUTPUT_A,
ir_sensor_port: str = INPUT_4,
ir_beacon_channel: int = 1,
touch_sensor_port: str = INPUT_1):
self.tank_driver = MoveTank(left_motor_port=left_leg_motor_port,
right_motor_port=right_leg_motor_port,
motor_class=LargeMotor)
self.shooting_motor = MediumMotor(address=shooting_motor_port)
self.touch_sensor = TouchSensor(address=touch_sensor_port)
self.ir_sensor = InfraredSensor(address=ir_sensor_port)
self.ir_beacon_channel = ir_beacon_channel
self.speaker = Sound()
def drive_once_by_ir_beacon(self, channel: int = 1, speed: float = 100):
if self.ir_sensor.top_left(
channel=self.ir_beacon_channel) and self.ir_sensor.top_right(
channel=self.ir_beacon_channel):
# go forward
self.tank_driver.on_for_seconds(left_speed=-speed,
right_speed=0,
seconds=1,
brake=True,
block=True)
self.tank_driver.on_for_seconds(left_speed=0,
right_speed=-speed,
seconds=1,
brake=True,
block=True)
elif self.ir_sensor.bottom_left(channel=self.ir_beacon_channel
) and self.ir_sensor.bottom_right(
channel=self.ir_beacon_channel):
# go backward
self.tank_driver.on_for_seconds(left_speed=speed,
right_speed=0,
seconds=1,
brake=True,
block=True)
self.tank_driver.on_for_seconds(left_speed=0,
right_speed=speed,
seconds=1,
brake=True,
block=True)
elif self.ir_sensor.top_left(channel=self.ir_beacon_channel
) and self.ir_sensor.bottom_right(
channel=self.ir_beacon_channel):
# turn around left
self.tank_driver.on_for_seconds(left_speed=0,
right_speed=-speed,
seconds=1,
brake=True,
block=True)
self.tank_driver.on_for_seconds(left_speed=speed,
right_speed=0,
seconds=1,
brake=True,
block=True)
elif self.ir_sensor.top_right(
channel=self.ir_beacon_channel) and self.ir_sensor.bottom_left(
channel=self.ir_beacon_channel):
# turn around right
self.tank_driver.on_for_seconds(left_speed=-speed,
right_speed=0,
seconds=1,
brake=True,
block=True)
self.tank_driver.on_for_seconds(left_speed=0,
right_speed=speed,
seconds=1,
brake=True,
block=True)
elif self.ir_sensor.top_left(channel=self.ir_beacon_channel):
# turn left
self.tank_driver.on(left_speed=0, right_speed=-speed)
elif self.ir_sensor.top_right(channel=self.ir_beacon_channel):
# turn right
self.tank_driver.on(left_speed=-speed, right_speed=0)
elif self.ir_sensor.bottom_left(channel=self.ir_beacon_channel):
# left backward
self.tank_driver.on(left_speed=0, right_speed=speed)
elif self.ir_sensor.bottom_right(channel=self.ir_beacon_channel):
# right backward
self.tank_driver.on(left_speed=speed, right_speed=0)
else:
self.tank_driver.off(brake=False)
def shoot_objects_by_ir_beacon(self, channel: int = 1, speed: float = 1):
if self.ir_sensor.beacon(channel=channel):
self.shooting_motor.on_for_rotations(speed=speed,
rotations=6,
block=True,
brake=True)
while self.ir_sensor.beacon(channel=channel):
pass
else:
self.shooting_motor.off(brake=False)
def seek_the_fruit(self, distance: float = 10):
if self.ir_sensor.proximity <= distance:
self.speaker.play_file(wav_file='/home/robot/sound/Fanfare.wav',
volume=100,
play_type=Sound.PLAY_WAIT_FOR_COMPLETE)
def run_if_chased(self, speed: float = 100, how_many_steps: int = 3):
if self.touch_sensor.is_pressed:
# go forward
for i in range(how_many_steps):
self.tank_driver.on_for_seconds(left_speed=-speed,
right_speed=0,
seconds=1,
brake=True,
block=True)
self.tank_driver.on_for_seconds(left_speed=0,
right_speed=-speed,
seconds=1,
brake=True,
block=True)
time.sleep(1)
# Make car do 180 and run over rest of crater
# wheels.on(-187, SpeedPercent(15))
# wheelsV2.on_for_distance(15, 50)
# wheels.on(187, SpeedPercent(15))
# wheelsV2.on_for_distance(15, -50)
wheels.on(-85, SpeedPercent(15))
while gyro.angle > -85:
print('Gyro angle {}'.format(gyro.angle))
wheels.on(0, SpeedPercent(25))
while proximity_sensor.distance_centimeters >= 16:
pass
# wheels.on(-100, SpeedPercent(-50))
# start_angleV2 = gyro.angle
# while gyro.angle > -80:
# print(gyro.angle)
# wheelsV2.on_for_distance(50, 100)
# wheelsV2.on_arc_left(50, 80)
wheels.off(brake=False)
extend_arm.off(brake=False)
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")
def inches_to_mm(self, inches):
"""
Convert inches to mm
"""
return inches*25.4
# For the arm motor, we want positive values to be up and negative to be down.
# The way that we're configured, we have to reverse the polarity to achieve this.
arm_motor = MediumMotor(address=ARM_MOTOR_PORT)
arm_motor.polarity = MediumMotor.POLARITY_INVERSED
# Let's initialize our arm motor by putting it all the way down until it
# stalls. With our build, it will stall when it hits the supporting arms
# for the color sensor and touch sensor (so it doesn't hit the ground - it's
# still slightly in the air.)
arm_motor.off()
arm_motor.on(speed=SpeedPercent(-75))
arm_motor.wait_until_not_moving(timeout=5000)
arm_motor.off()
# Initialize our drive base - MyMoveDifferential extends MoveDifferential, which is a 'tank-like'
# class. Then drive forward at 25% speed for 24.0 inches.
mdiff = MyMoveDifferential(left_motor_port=LEFT_MOTOR_PORT,
right_motor_port=RIGHT_MOTOR_PORT, wheel_class=EV3EducationSetTire,
wheel_distance_mm=AXLE_TRACK_MM)
mdiff.drive_straight_distance_inches(speed=SpeedPercent(25), distance_inches=24.0)
# Raise the arm until we stall, which will hopefully raise the lever
# at the base of the crane. Notice that we can specify a timeout should the
# motor not stall for whatever reason -- that way, we don't get stuck out on the game board
# and lose a token.
class Kraz33Hors3:
def __init__(self,
back_foot_motor_port: str = OUTPUT_B,
front_foot_motor_port: str = OUTPUT_C,
gear_motor_port: str = OUTPUT_A,
touch_sensor_port: str = INPUT_1,
color_sensor_port: str = INPUT_3,
ir_sensor_port: str = INPUT_4,
ir_beacon_channel: int = 1):
self.tank_driver = MoveTank(left_motor_port=back_foot_motor_port,
right_motor_port=front_foot_motor_port,
motor_class=LargeMotor)
self.gear_motor = MediumMotor(address=gear_motor_port)
self.touch_sensor = TouchSensor(address=touch_sensor_port)
self.color_sensor = ColorSensor(address=color_sensor_port)
self.ir_sensor = InfraredSensor(address=ir_sensor_port)
self.ir_beacon_channel = ir_beacon_channel
def drive_once_by_ir_beacon(self, speed: float = 100):
# forward
if self.ir_sensor.top_left(
channel=self.ir_beacon_channel) and self.ir_sensor.top_right(
channel=self.ir_beacon_channel):
self.tank_driver.on_for_seconds(left_speed=speed,
right_speed=-speed,
seconds=1,
brake=False,
block=True)
# backward
elif self.ir_sensor.bottom_left(channel=self.ir_beacon_channel
) and self.ir_sensor.bottom_right(
channel=self.ir_beacon_channel):
self.tank_driver.on_for_seconds(left_speed=-speed,
right_speed=speed,
seconds=1,
brake=False,
block=True)
# move crazily
elif self.ir_sensor.beacon(channel=self.ir_beacon_channel):
self.gear_motor.on(speed=speed, brake=False, block=False)
self.tank_driver.on_for_seconds(left_speed=-speed / 3,
right_speed=-speed / 3,
seconds=1,
brake=False,
block=True)
else:
self.gear_motor.off(brake=False)
def keep_driving_by_ir_beacon(self, speed: float = 100):
while True:
self.drive_once_by_ir_beacon(speed=speed)
def back_whenever_touched(self, speed: float = 100):
while True:
if self.touch_sensor.is_pressed:
self.tank_driver.on_for_seconds(left_speed=-speed,
right_speed=speed,
seconds=1,
brake=False,
block=True)
def main(self):
Process(target=self.back_whenever_touched, daemon=True).start()
self.keep_driving_by_ir_beacon()
wheels.on(0, SpeedPercent(50))
while not button.any():
if proximity.distance_centimeters < 5.0:
# swatter_sensor.scan_left()
# swatter_sensor.scan_right()
# if max(swatter_sensor.get_sensor_readings()) >= 250:
# wheels.on_for_rotations(100, SpeedPercent(50), 0.75)
# wheels.on_for_rotations(0, SpeedPercent(35), 1)
claw.on_for_rotations(SpeedPercent(50), -2)
wheels.on_for_seconds(0, SpeedPercent(-100), 1, block=True)
wheels.on_for_rotations(45, SpeedPercent(100), 0.5, block=True)
wheels.on_for_rotations(0, SpeedPercent(85), 5, block=True)
claw.on_for_rotations(SpeedPercent(50), 2)
wheels.on_for_rotations(0, SpeedPercent(-100), 1, block=True)
wheels.on_for_degrees(-100, SpeedPercent(100), 360)
elif touch.is_pressed:
wheels.on_for_rotations(0, SpeedPercent(-50), 1, block=True)
wheels.on_for_rotations(100, SpeedPercent(100), 1.5, block=True)
else:
wheels.on(0, SpeedPercent(50))
proc = Thread(target=grab_everything)
proc.start()
proc.join()
wheels.off(brake=False)
claw.off(brake=False)
brake=True,
block=True)
while True:
if IR_SENSOR.proximity < 30:
LIGHTS.set_color(
group=Leds.LEFT,
color=Leds.RED,
pct=1)
LIGHTS.set_color(
group=Leds.RIGHT,
color=Leds.RED,
pct=1)
MEDIUM_MOTOR.off(brake=True)
TAIL_MOTOR.off(brake=True)
SPEAKER.play_file(
wav_file='/home/robot/sound/Snake hiss.wav',
volume=100,
play_type=Sound.PLAY_NO_WAIT_FOR_COMPLETE)
CHEST_MOTOR.on_for_seconds(
speed=100,
seconds=1,
brake=True,
block=True)
MEDIUM_MOTOR.on(
class Kraz33Hors3:
def __init__(self,
back_foot_motor_port: str = OUTPUT_B,
front_foot_motor_port: str = OUTPUT_C,
gear_motor_port: str = OUTPUT_A,
touch_sensor_port: str = INPUT_1,
color_sensor_port: str = INPUT_3,
ir_sensor_port: str = INPUT_4,
ir_beacon_channel: int = 1):
self.tank_driver = MoveTank(left_motor_port=back_foot_motor_port,
right_motor_port=front_foot_motor_port,
motor_class=LargeMotor)
self.gear_motor = MediumMotor(address=gear_motor_port)
self.touch_sensor = TouchSensor(address=touch_sensor_port)
self.color_sensor = ColorSensor(address=color_sensor_port)
self.ir_sensor = InfraredSensor(address=ir_sensor_port)
self.ir_beacon_channel = ir_beacon_channel
def drive_once_by_ir_beacon(self, speed: float = 100):
# forward
if self.ir_sensor.top_left(
channel=self.ir_beacon_channel) and self.ir_sensor.top_right(
channel=self.ir_beacon_channel):
self.tank_driver.on_for_seconds(left_speed=speed,
right_speed=-speed,
seconds=1,
brake=False,
block=True)
# backward
elif self.ir_sensor.bottom_left(channel=self.ir_beacon_channel
) and self.ir_sensor.bottom_right(
channel=self.ir_beacon_channel):
self.tank_driver.on_for_seconds(left_speed=-speed,
right_speed=speed,
seconds=1,
brake=False,
block=True)
# move crazily
elif self.ir_sensor.beacon(channel=self.ir_beacon_channel):
self.gear_motor.on(speed=speed, brake=False, block=False)
self.tank_driver.on_for_seconds(left_speed=-speed / 3,
right_speed=-speed / 3,
seconds=1,
brake=False,
block=True)
else:
self.gear_motor.off(brake=False)
def keep_driving_by_ir_beacon(self, speed: float = 100):
while True:
self.drive_once_by_ir_beacon(speed=speed)
def back_whenever_touched(self, speed: float = 100):
while True:
if self.touch_sensor.is_pressed:
self.tank_driver.on_for_seconds(left_speed=-speed,
right_speed=speed,
seconds=1,
brake=False,
block=True)
def main(self):
# FIXME: when this thread is activated, the program encounters OSError after a while:
# Traceback (most recent call last):
# File "/home/robot/Kraz33-Mov3r/Kraz33-Mov3r.EV3Dev2.MicroPython.Threading.FIXME.py", line 92, in <module>
# File "/home/robot/Kraz33-Mov3r/Kraz33-Mov3r.EV3Dev2.MicroPython.Threading.FIXME.py", line 86, in main
# File "/home/robot/Kraz33-Mov3r/Kraz33-Mov3r.EV3Dev2.MicroPython.Threading.FIXME.py", line 68, in keep_driving_by_ir_beacon
# File "/home/robot/Kraz33-Mov3r/Kraz33-Mov3r.EV3Dev2.MicroPython.Threading.FIXME.py", line 43, in drive_once_by_ir_beacon
# File "ev3dev2/motor.py", line 1957, in on_for_rotations
# File "ev3dev2/motor.py", line 1945, in on_for_degrees
# File "ev3dev2/motor.py", line 1803, in _block
# File "ev3dev2/motor.py", line 1787, in wait_until_not_moving
# File "ev3dev2/motor.py", line 928, in wait_until_not_moving
# File "ev3dev2/motor.py", line 908, in wait
# OSError: 4
Thread(target=self.back_whenever_touched).start()
self.keep_driving_by_ir_beacon()
debug_print("motor stop")
motor_thread = MotorThread()
motor_thread.setDaemon(True)
motor_thread.start()
for event in gamepad.read_loop(): #this loops infinitely
if event.type == 1 and event.code == 304: ## Cross-button
if event.value == 1:
if not ladle.is_running:
debug_print("Ladle down start")
ladle.on(-50)
else:
debug_print("Ladle down stop")
ladle.off()
if event.type == 1 and event.code == 305 and event.value == 1:
debug_print("Round button is pressed")
if event.type == 1 and event.code == 308 and event.value == 1:
debug_print("Square button is pressed")
if event.type == 1 and event.code == 307: ## Triangle-button
if event.value == 1:
if not ladle.is_running:
debug_print("Ladle up start")
ladle.on(50)
else:
debug_print("Ladle up stop")
ladle.off()
if event.type == 1 and event.code == 544 and event.value == 1:
debug_print("Up button is pressed")
#!/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 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()
class Gripp3r:
def __init__(self,
left_motor_port: str = OUTPUT_B,
right_motor_port: str = OUTPUT_C,
grip_motor_port: str = OUTPUT_A,
touch_sensor_port: str = INPUT_1,
ir_sensor_port: str = INPUT_4,
ir_beacon_channel: int = 1):
self.tank_driver = MoveTank(left_motor_port=left_motor_port,
right_motor_port=right_motor_port,
motor_class=LargeMotor)
self.grip_motor = MediumMotor(address=grip_motor_port)
self.touch_sensor = TouchSensor(address=touch_sensor_port)
self.ir_sensor = InfraredSensor(address=ir_sensor_port)
self.ir_beacon_channel = ir_beacon_channel
self.speaker = Sound()
def drive_once_by_ir_beacon(self, speed: float = 100):
if self.ir_sensor.top_left(channel=self.ir_beacon_channel) and \
self.ir_sensor.top_right(channel=self.ir_beacon_channel):
# go forward
self.tank_driver.on(left_speed=speed, right_speed=speed)
elif self.ir_sensor.bottom_left(channel=self.ir_beacon_channel) and \
self.ir_sensor.bottom_right(channel=self.ir_beacon_channel):
# go backward
self.tank_driver.on(left_speed=-speed, right_speed=-speed)
elif self.ir_sensor.top_left(channel=self.ir_beacon_channel) and \
self.ir_sensor.bottom_right(channel=self.ir_beacon_channel):
# turn around left
self.tank_driver.on(left_speed=-speed, right_speed=speed)
elif self.ir_sensor.top_right(channel=self.ir_beacon_channel) and \
self.ir_sensor.bottom_left(channel=self.ir_beacon_channel):
# turn around right
self.tank_driver.on(left_speed=speed, right_speed=-speed)
elif self.ir_sensor.top_left(channel=self.ir_beacon_channel):
# turn left
self.tank_driver.on(left_speed=0, right_speed=speed)
elif self.ir_sensor.top_right(channel=self.ir_beacon_channel):
# turn right
self.tank_driver.on(left_speed=speed, right_speed=0)
elif self.ir_sensor.bottom_left(channel=self.ir_beacon_channel):
# left backward
self.tank_driver.on(left_speed=0, right_speed=-speed)
elif self.ir_sensor.bottom_right(channel=self.ir_beacon_channel):
# right backward
self.tank_driver.on(left_speed=-speed, right_speed=0)
else:
self.tank_driver.off(brake=False)
def grip_or_release_by_ir_beacon(self, speed: float = 50):
if self.ir_sensor.beacon(channel=self.ir_beacon_channel):
if self.touch_sensor.is_pressed:
self.speaker.play_file(
wav_file='/home/robot/sound/Air release.wav',
volume=100,
play_type=Sound.PLAY_NO_WAIT_FOR_COMPLETE)
self.grip_motor.on_for_seconds(speed=speed,
seconds=1,
brake=True,
block=True)
else:
self.speaker.play_file(
wav_file='/home/robot/sound/Airbrake.wav',
volume=100,
play_type=Sound.PLAY_NO_WAIT_FOR_COMPLETE)
self.grip_motor.on(speed=-speed, brake=False, block=False)
self.touch_sensor.wait_for_pressed()
self.grip_motor.off(brake=True)
while self.ir_sensor.beacon(channel=self.ir_beacon_channel):
pass
def main(self, speed: float = 100):
self.grip_motor.on_for_seconds(speed=-50,
seconds=1,
brake=True,
block=True)
while True:
self.drive_once_by_ir_beacon(speed=speed)
self.grip_or_release_by_ir_beacon()
#!/usr/bin/env micropython
from ev3dev2.motor import MediumMotor, OUTPUT_A
from ev3dev2.sensor.lego import InfraredSensor
from ev3dev2.sensor import INPUT_4
from time import sleep
import math
import sys
#Code is here
IR = InfraredSensor(INPUT_4)
meMotor = MediumMotor(OUTPUT_A)
x = 0
while x < 1000:
BeaconHeading = IR.heading_and_distance(channel=1)
print(BeaconHeading, file=sys.stderr)
(Heading, Distance) = BeaconHeading
print(Heading, file=sys.stderr)
if Heading:
print(Heading, file=sys.stderr)
Heading = Heading * 2
meMotor.on(speed=Heading)
else:
meMotor.off()
sleep(.01)
#!/usr/bin/env python3
from ev3dev2.motor import MediumMotor, LargeMotor, OUTPUT_A, OUTPUT_B, SpeedPercent, MoveTank
from ev3dev2.sensor import INPUT_1
from ev3dev2.sensor.lego import TouchSensor
from ev3dev2.led import Leds
from ev3dev2.sound import Sound
ts = TouchSensor()
leds = Leds()
m = MediumMotor(OUTPUT_A)
Sound.speak("Hello, I am a robot! Awaiting your command...")
while True:
if ts.is_pressed:
leds.set_color("LEFT", "GREEN")
leds.set_color("RIGHT", "GREEN")
m.on(speed=45)
else:
leds.set_color("LEFT", "RED")
leds.set_color("RIGHT", "RED")
m.off()
