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 Gripper:
def __init__(self):
self.rc = rc.RobotContainer()
self.motor = MediumMotor(self.rc.GRIPPER)
self.color1 = ColorSensor(self.rc.COLOR_RECOGNITION)
self.color2 = ColorSensor(self.rc.COLOR_RECOGNITION2)
def lowerMotor(self, speed):
self.motor.on(speed, block=False)
self.motor.wait_until_not_moving(300)
def getColors(self):
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
def blue():
tank = MoveTank(OUTPUT_A, OUTPUT_B)
m = MediumMotor(OUTPUT_C)
now = ColorSensor()
tank.on_for_rotations(50,50,0.46) # turn 90 degrees clock wise
#lineTrace()
tank.on_for_rotations(10, 10, 0.1)
while True:
if now.color == 1:
break
else:
m.on(50, 50)
return True
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
from ev3dev2.motor import LargeMotor, MediumMotor, SpeedPercent, MoveTank, OUTPUT_A, OUTPUT_B, OUTPUT_C, OUTPUT_D from ev3dev2.sensor.lego import TouchSensor #, GyroSensor, ColorSensor # from ev3dev2.led import Leds import time # from math import cos, radians, degrees # import os # import sys tank_drive = MoveTank(OUTPUT_A, OUTPUT_B) front_motor = MediumMotor(OUTPUT_C) top_motor = MediumMotor(OUTPUT_D) # gs = GyroSensor() # leds = Leds() ts = TouchSensor() ratio_degrees_to_inches = 360 / 8.44 rotate = 135.0 / 90.0 # On the spot mission start tank_drive.on_for_degrees(SpeedPercent(40), SpeedPercent(40), ratio_degrees_to_inches * 22, brake=True) tank_drive.on_for_degrees(SpeedPercent(-15), SpeedPercent(15), rotate * 96, brake=True) tank_drive.on_for_degrees(SpeedPercent(40), SpeedPercent(40), ratio_degrees_to_inches * 30, brake=True) front_motor.on(speed=SpeedPercent(-20)) time.sleep(0.5) front_motor.off() top_motor.on(speed=SpeedPercent(-20)) time.sleep(0.5) top_motor.off()
#!/usr/bin/env python3
from ev3dev2.motor import OUTPUT_D, MediumMotor
spinner = MediumMotor(OUTPUT_D)
spinner.on(-10)
while True:
pass
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)
from ev3dev2.motor import LargeMotor, MediumMotor, MoveTank, MoveSteering, OUTPUT_A, OUTPUT_B, OUTPUT_C
from ev3dev2.sound import Sound
from time import sleep
MEDIUM_MOTOR = MediumMotor(address=OUTPUT_A)
TANK_DRIVER = MoveTank(left_motor_port=OUTPUT_B,
right_motor_port=OUTPUT_C,
motor_class=LargeMotor)
STEER_DRIVER = MoveSteering(left_motor_port=OUTPUT_B,
right_motor_port=OUTPUT_C,
motor_class=LargeMotor)
SPEAKER = Sound()
MEDIUM_MOTOR.on(speed=-100, brake=False, block=False)
for i in range(2):
TANK_DRIVER.on_for_rotations(left_speed=75,
right_speed=75,
rotations=2,
brake=True,
block=True)
MEDIUM_MOTOR.on(speed=100, brake=False, block=False)
SPEAKER.play_file(wav_file='/home/robot/sound/Airbrake.wav',
volume=100,
play_type=Sound.PLAY_WAIT_FOR_COMPLETE)
sleep(0.5)
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()
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 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 python3
from time import sleep
from ev3dev2.motor import OUTPUT_A, OUTPUT_B, OUTPUT_C, OUTPUT_D, SpeedRPM, MediumMotor
from ev3dev2.led import Leds
mA = MediumMotor(OUTPUT_A)
mB = MediumMotor(OUTPUT_B)
leds = Leds()
leds.all_off()
print("Robot Starting")
mA.on(SpeedRPM(-80))
mB.on(SpeedRPM(80))
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)
if( steps < 0 ):
steps = 0
elif(btn.check_buttons(buttons=['enter'])):
break
# update the steps on screen
lcd.text_pixels( str(steps), True, 80, 50, font='courB18')
lcd.update()
# btn.wait_for_released(buttons=['up','down', 'left', 'right','enter'])
log.info('Climbing ......')
while( steps > 0 ):
oneStep()
steps -= 1
# update the steps on screen
lcd.text_pixels( str(steps), True, 80, 50, font='courB18')
lcd.update()
# move forward & stop
mm_move.on(-90)
lm_move.on(-100)
sleep(0.5)
mm_move.stop()
lm_move.stop()
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")
medMot.on(-20)
while True:
if bttn.enter == 1:
break
medMot.stop()
#!/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 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)
#!/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()
"""
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.
arm_motor.off()
"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()
#!/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 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")
# lastSpeed = speed
#self.motor.stop()
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 follow_line:
counter_max += counter_max
motor_thread = MotorThread()
motor_thread.setDaemon(True)
motor_thread.start()
claw_thread = ClawThread()
claw_thread.setDaemon(True)
claw_thread.start()
line_thread = LineFollowThread()
line_thread.setDaemon(True)
line_thread.start()
duck = MediumMotor(OUTPUT_A)
duck.on(20)
for event in gamepad.read_loop(): # this loops infinitely
if event.type == 2 or event.type == 1 or event.type == 0:
if event.value != 0:
print("%s %s %s" % (event.type, event.code, event.value))
if event.type == 3: # A stick is moved
# 5: R2
# 2: L2
# 0: L-stick X-axis
# 1: L-stick Y-axis
# 3: R-stick X-axis
# 4: R-stick Y-axis
# 304: X-button
# 305: O-button
class Robot:
def __init__(self):
self.sound = Sound()
self.direction_motor = MediumMotor(OUTPUT_D)
self.swing_motorL = LargeMotor(OUTPUT_A)
self.swing_motorC = LargeMotor(OUTPUT_B)
self.swing_motorR = LargeMotor(OUTPUT_C)
self.swing_motors = [
self.swing_motorL, self.swing_motorC, self.swing_motorR
]
self.touch_sensor = TouchSensor(INPUT_1)
self.console = Console(DEFAULT_FONT)
self.buttons = Button()
self.beeps_enabled = True
def beep(self, frequency=700, wait_for_comeplete=False):
if not self.beeps_enabled:
return
play_type = Sound.PLAY_WAIT_FOR_COMPLETE if wait_for_comeplete else Sound.PLAY_NO_WAIT_FOR_COMPLETE
self.sound.beep("-f %i" % frequency, play_type=play_type)
def calibrate_dir(self):
''' Calibrate direction motor '''
# Run motor with 25% power, and wait until it stops running
self.direction_motor.on(SpeedPercent(-10), block=False)
# while (not self.direction_motor.STATE_OVERLOADED):
# print(self.direction_motor.duty_cycle)
self.direction_motor.wait_until(self.direction_motor.STATE_OVERLOADED)
self.direction_motor.stop_action = Motor.STOP_ACTION_COAST
self.direction_motor.stop()
time.sleep(.5)
# Reset to straight
# self.direction_motor.on_for_seconds(SpeedPercent(10), .835, brake=True, block=True)
self.direction_motor.on_for_degrees(SpeedPercent(10),
127,
brake=True,
block=True)
self.direction_motor.reset()
print("Motor reset, position: " + str(self.direction_motor.position))
time.sleep(.5)
def calibrate_swing(self):
for m in self.swing_motors:
m.stop_action = m.STOP_ACTION_HOLD
m.on(SpeedPercent(6))
self.swing_motorC.wait_until(self.swing_motorC.STATE_OVERLOADED, 2000)
for m in self.swing_motors:
m.stop_action = m.STOP_ACTION_HOLD
m.on_for_degrees(SpeedPercent(5), -15, brake=True, block=False)
self.swing_motorC.wait_while('running')
for m in self.swing_motors:
m.reset()
m.stop_action = m.STOP_ACTION_HOLD
m.stop()
print("Ready Angle: %i" % self.swing_motorC.position)
def ready_swing(self, angle: int):
right_angle = -(angle / 3)
# adjust motors to target angle
for m in self.swing_motors:
m.stop_action = Motor.STOP_ACTION_HOLD
m.on_for_degrees(SpeedPercent(2),
right_angle,
brake=True,
block=False)
self.swing_motorC.wait_while('running')
for m in self.swing_motors:
m.stop_action = Motor.STOP_ACTION_HOLD
m.stop()
print("Swing Angle: %i" % self.swing_motorC.position)
def set_direction(self, direction):
print("Setting direction to: " + str(direction))
#direction = self.__aim_correction(direction)
self.direction_motor.on_for_degrees(SpeedPercent(10),
round(direction * 3))
print("Direction set to: " + str(self.direction_motor.position))
#
# def __aim_correction(self, direction):
# x = direction
# y = -0.00000000429085685725*x**6 + 0.00000004144345630728*x**5 + 0.00001219331494759860*x**4 + 0.00020702946527870400*x**3 + 0.00716486965517779000*x**2 + 1.29675836037884000000*x + 0.27064829453014400000
#
# return y
def shoot(self, power):
print("SHOOT, power: %i" % power)
for m in self.swing_motors:
m.duty_cycle_sp = -power
for m in self.swing_motors:
m.run_direct()
time.sleep(.5)
self.swing_motorC.wait_until_not_moving()
for m in self.swing_motors:
m.reset()
def wait_for_button(self):
self.touch_sensor.wait_for_bump()
def __set_display(self, str):
self.console.set_font(font=LARGER_FONT, reset_console=True)
self.console.text_at(str, column=1, row=1, reset_console=True)
def wait_for_power_select(self, power=0, angle=0, steps=1):
self.__set_display("Pow: %i\nAngle: %i" % (power, angle))
def left():
power -= steps
if power < 0:
power = 0
self.__set_display("Pow: %i\nAngle: %i" % (power, angle))
self.buttons.wait_for_released(buttons=['left'], timeout_ms=150)
def right():
power += steps
if power > 100:
power = 100
self.__set_display("Pow: %i\nAngle: %i" % (power, angle))
self.buttons.wait_for_released(buttons=['right'], timeout_ms=150)
def up():
angle += steps
if angle > 110:
angle = 110
self.__set_display("Pow: %i\nAngle: %i" % (power, angle))
self.buttons.wait_for_released(buttons=['up'], timeout_ms=150)
def down():
angle -= steps
if angle < 0:
angle = 0
self.__set_display("Pow: %i\nAngle: %i" % (power, angle))
self.buttons.wait_for_released(buttons=['down'], timeout_ms=150)
while not self.touch_sensor.is_pressed:
if self.buttons.left:
left()
elif self.buttons.right:
right()
elif self.buttons.up:
up()
elif self.buttons.down:
down()
self.console.set_font(font=DEFAULT_FONT, reset_console=True)
return (power, angle)
def select_connection_mode(self):
self.__set_display("Enable Connection\nLeft: True - Right: False")
enabled = True
while not self.touch_sensor.is_pressed:
if self.buttons.left:
enabled = True
self.buttons.wait_for_released(buttons=['left'])
break
elif self.buttons.right:
enabled = False
self.buttons.wait_for_released(buttons=['right'])
break
self.console.set_font(font=DEFAULT_FONT, reset_console=True)
return enabled
def print(self, string):
print(string)
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()
#!/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 OUTPUT_D, MediumMotor
spinner = MediumMotor(OUTPUT_D)
spinner.on(10)
while True:
pass
