class EV3D4RemoteControlled(RemoteControlledTank):
def __init__(self,
medium_motor=OUTPUT_A,
left_motor=OUTPUT_C,
right_motor=OUTPUT_B):
RemoteControlledTank.__init__(self, left_motor, right_motor)
self.medium_motor = MediumMotor(medium_motor)
self.medium_motor.reset()
def doLifter():
"""Test code for operating lifter"""
lifter = MediumMotor()
lifter.reset()
lifter.run_to_rel_pos(position_sp=60, speed_sp=100, stop_action='hold')
lifter.wait_while('running')
sleep(1)
lifter.run_to_rel_pos(position_sp=-60, speed_sp=100, stop_action='hold')
lifter.wait_while('running')
class El3ctricGuitar:
NOTES = [1318, 1174, 987, 880, 783, 659, 587, 493, 440, 392, 329, 293]
N_NOTES = len(NOTES)
def __init__(
self, lever_motor_port: str = OUTPUT_D,
touch_sensor_port: str = INPUT_1, ir_sensor_port: str = INPUT_4):
self.lever_motor = MediumMotor(address=lever_motor_port)
self.touch_sensor = TouchSensor(address=touch_sensor_port)
self.ir_sensor = InfraredSensor(address=ir_sensor_port)
self.leds = Leds()
self.speaker = Sound()
def start_up(self):
self.leds.animate_flash(
color='ORANGE',
groups=('LEFT', 'RIGHT'),
sleeptime=0.5,
duration=3,
block=True)
self.lever_motor.on_for_seconds(
speed=5,
seconds=1,
brake=False,
block=True)
self.lever_motor.on_for_degrees(
speed=-5,
degrees=30,
brake=True,
block=True)
sleep(0.1)
self.lever_motor.reset()
def play_music(self):
if self.touch_sensor.is_released:
raw = sum(self.ir_sensor.proximity for _ in range(4)) / 4
self.speaker.tone(
self.NOTES[min(round(raw / 5), self.N_NOTES - 1)]
- 11 * self.lever_motor.position,
100,
play_type=Sound.PLAY_WAIT_FOR_COMPLETE)
def main(self):
self.start_up()
while True:
self.play_music()
class TRACK3R(RemoteControlledTank):
"""
Base class for all TRACK3R variations. The only difference in the child
classes are in how the medium motor is handled.
To enable the medium motor toggle the beacon button on the EV3 remote.
"""
def __init__(self, medium_motor, left_motor, right_motor):
RemoteControlledTank.__init__(self, left_motor, right_motor)
self.medium_motor = MediumMotor(medium_motor)
self.medium_motor.reset()
class EV3D4WebControlled(WebControlledTank):
def __init__(self,
medium_motor=OUTPUT_A,
left_motor=OUTPUT_C,
right_motor=OUTPUT_B):
WebControlledTank.__init__(self, left_motor, right_motor)
self.medium_motor = MediumMotor(medium_motor)
if not self.medium_motor.connected:
log.error("%s is not connected" % self.medium_motor)
sys.exit(1)
self.medium_motor.reset()
class EV3D4RemoteControlled(RemoteControlledTank):
def __init__(
self,
left_motor_port: str = OUTPUT_B, right_motor_port: str = OUTPUT_C,
medium_motor_port: str = OUTPUT_A,
speed: float = 400,
ir_beacon_channel: int = 1):
super().__init__(
left_motor=left_motor_port, right_motor=right_motor_port,
polarity='inversed',
speed=speed,
channel=ir_beacon_channel)
self.medium_motor = MediumMotor(address=medium_motor_port)
self.medium_motor.reset()
m2.on(-15)
while g.angle - g_off > degrees - 8:
if command_handler(bob.commands):
reset_motors()
return
f.write("offset: {} angle: {} delta: {}\n".format(
g_off, g.angle, g.angle - g_off))
m1.on((10 - (5 * (g.angle - g_off) / degrees)))
m2.on((-10 + (5 * (g.angle - g_off) / degrees)))
m1.off()
m2.off()
g_off = 0
time.sleep(1)
m4.reset()
def dance():
global bob
while True:
m1.on(100)
m2.on(-100)
if command_handler(bob.commands):
reset_motors()
return
time.sleep(0.5)
m1.on(-100)
m2.on(100)
time.sleep(0.5)
LeftAction = MediumMotor(OUTPUT_A)
RightAction = MediumMotor(OUTPUT_D)
LeftWheel = LargeMotor(OUTPUT_B)
RightWheel = LargeMotor(OUTPUT_C)
btn = Button()
contin = True
print('*** ATHENA ROCKS! ***')
print('1 - Left for Trip 1')
print('2 - Up for Trip 2')
print('3 - Right for Trip 3')
print('4 - Down for Trip 4')
print('5 - Center for Trip 5')
sound.beep()
while contin:
RightAction.reset()
LeftAction.reset()
LeftWheel.reset()
RightWheel.reset()
if btn.left == True:
runTrip1()
sound.beep()
elif btn.up == True:
runTrip2()
sound.beep()
elif btn.right == True:
runTrip3()
sound.beep()
elif btn.down == True:
runTrip4()
sound.beep()
class MindCuber(object):
scan_order = [
5, 9, 6, 3, 2, 1, 4, 7, 8, 23, 27, 24, 21, 20, 19, 22, 25, 26, 50, 54,
51, 48, 47, 46, 49, 52, 53, 14, 10, 13, 16, 17, 18, 15, 12, 11, 41, 43,
44, 45, 42, 39, 38, 37, 40, 32, 34, 35, 36, 33, 30, 29, 28, 31
]
hold_cube_pos = 85
rotate_speed = 400
flip_speed = 450
flip_speed_push = 400
def __init__(self):
self.shutdown = False
self.flipper = LargeMotor(OUTPUT_A)
self.turntable = LargeMotor(OUTPUT_B)
self.colorarm = MediumMotor(OUTPUT_C)
self.color_sensor = ColorSensor()
self.color_sensor.mode = self.color_sensor.MODE_RGB_RAW
self.infrared_sensor = InfraredSensor()
self.init_motors()
self.state = ['U', 'D', 'F', 'L', 'B', 'R']
self.rgb_solver = None
signal.signal(signal.SIGTERM, self.signal_term_handler)
signal.signal(signal.SIGINT, self.signal_int_handler)
filename_max_rgb = 'max_rgb.txt'
if os.path.exists(filename_max_rgb):
with open(filename_max_rgb, 'r') as fh:
for line in fh:
(color, value) = line.strip().split()
if color == 'red':
self.color_sensor.red_max = int(value)
log.info("red max is %d" % self.color_sensor.red_max)
elif color == 'green':
self.color_sensor.green_max = int(value)
log.info("green max is %d" %
self.color_sensor.green_max)
elif color == 'blue':
self.color_sensor.blue_max = int(value)
log.info("blue max is %d" % self.color_sensor.blue_max)
def init_motors(self):
for x in (self.flipper, self.turntable, self.colorarm):
x.reset()
log.info("Initialize flipper %s" % self.flipper)
self.flipper.on(SpeedDPS(-50), block=True)
self.flipper.off()
self.flipper.reset()
log.info("Initialize colorarm %s" % self.colorarm)
self.colorarm.on(SpeedDPS(500), block=True)
self.colorarm.off()
self.colorarm.reset()
log.info("Initialize turntable %s" % self.turntable)
self.turntable.off()
self.turntable.reset()
def shutdown_robot(self):
log.info('Shutting down')
self.shutdown = True
if self.rgb_solver:
self.rgb_solver.shutdown = True
for x in (self.flipper, self.turntable, self.colorarm):
# We are shutting down so do not 'hold' the motors
x.stop_action = 'brake'
x.off(False)
def signal_term_handler(self, signal, frame):
log.error('Caught SIGTERM')
self.shutdown_robot()
def signal_int_handler(self, signal, frame):
log.error('Caught SIGINT')
self.shutdown_robot()
def apply_transformation(self, transformation):
self.state = [self.state[t] for t in transformation]
def rotate_cube(self, direction, nb):
current_pos = self.turntable.position
final_pos = 135 * round(
(self.turntable.position + (270 * direction * nb)) / 135.0)
log.info(
"rotate_cube() direction %s, nb %s, current_pos %d, final_pos %d" %
(direction, nb, current_pos, final_pos))
if self.flipper.position > 35:
self.flipper_away()
self.turntable.on_to_position(SpeedDPS(MindCuber.rotate_speed),
final_pos)
if nb >= 1:
for i in range(nb):
if direction > 0:
transformation = [0, 1, 5, 2, 3, 4]
else:
transformation = [0, 1, 3, 4, 5, 2]
self.apply_transformation(transformation)
def rotate_cube_1(self):
self.rotate_cube(1, 1)
def rotate_cube_2(self):
self.rotate_cube(1, 2)
def rotate_cube_3(self):
self.rotate_cube(-1, 1)
def rotate_cube_blocked(self, direction, nb):
# Move the arm down to hold the cube in place
self.flipper_hold_cube()
# OVERROTATE depends on lot on MindCuber.rotate_speed
current_pos = self.turntable.position
OVERROTATE = 18
final_pos = int(135 * round(
(current_pos + (270 * direction * nb)) / 135.0))
temp_pos = int(final_pos + (OVERROTATE * direction))
log.info(
"rotate_cube_blocked() direction %s nb %s, current pos %s, temp pos %s, final pos %s"
% (direction, nb, current_pos, temp_pos, final_pos))
self.turntable.on_to_position(SpeedDPS(MindCuber.rotate_speed),
temp_pos)
self.turntable.on_to_position(SpeedDPS(MindCuber.rotate_speed / 4),
final_pos)
def rotate_cube_blocked_1(self):
self.rotate_cube_blocked(1, 1)
def rotate_cube_blocked_2(self):
self.rotate_cube_blocked(1, 2)
def rotate_cube_blocked_3(self):
self.rotate_cube_blocked(-1, 1)
def flipper_hold_cube(self, speed=300):
current_position = self.flipper.position
# Push it forward so the cube is always in the same position
# when we start the flip
if (current_position <= MindCuber.hold_cube_pos - 10
or current_position >= MindCuber.hold_cube_pos + 10):
self.flipper.ramp_down_sp = 400
self.flipper.on_to_position(SpeedDPS(speed),
MindCuber.hold_cube_pos)
sleep(0.05)
def flipper_away(self, speed=300):
"""
Move the flipper arm out of the way
"""
log.info("flipper_away()")
self.flipper.ramp_down_sp = 400
self.flipper.on_to_position(SpeedDPS(speed), 0)
def flip(self):
"""
Motors will sometimes stall if you call on_to_position() multiple
times back to back on the same motor. To avoid this we call a 50ms
sleep in flipper_hold_cube() and after each on_to_position() below.
We have to sleep after the 2nd on_to_position() because sometimes
flip() is called back to back.
"""
log.info("flip()")
if self.shutdown:
return
# Move the arm down to hold the cube in place
self.flipper_hold_cube()
# Grab the cube and pull back
self.flipper.ramp_up_sp = 200
self.flipper.ramp_down_sp = 0
self.flipper.on_to_position(SpeedDPS(self.flip_speed), 190)
sleep(0.05)
# At this point the cube is at an angle, push it forward to
# drop it back down in the turntable
self.flipper.ramp_up_sp = 200
self.flipper.ramp_down_sp = 400
self.flipper.on_to_position(SpeedDPS(self.flip_speed_push),
MindCuber.hold_cube_pos)
sleep(0.05)
transformation = [2, 4, 1, 3, 0, 5]
self.apply_transformation(transformation)
def colorarm_middle(self):
log.info("colorarm_middle()")
self.colorarm.on_to_position(SpeedDPS(600), -750)
def colorarm_corner(self, square_index):
"""
The lower the number the closer to the center
"""
log.info("colorarm_corner(%d)" % square_index)
position_target = -580
if square_index == 1:
position_target -= 10
elif square_index == 3:
position_target -= 30
elif square_index == 5:
position_target -= 20
elif square_index == 7:
pass
else:
raise ScanError(
"colorarm_corner was given unsupported square_index %d" %
square_index)
self.colorarm.on_to_position(SpeedDPS(600), position_target)
def colorarm_edge(self, square_index):
"""
The lower the number the closer to the center
"""
log.info("colorarm_edge(%d)" % square_index)
position_target = -640
if square_index == 2:
position_target -= 20
elif square_index == 4:
position_target -= 40
elif square_index == 6:
position_target -= 20
elif square_index == 8:
pass
else:
raise ScanError(
"colorarm_edge was given unsupported square_index %d" %
square_index)
self.colorarm.on_to_position(SpeedDPS(600), position_target)
def colorarm_remove(self):
log.info("colorarm_remove()")
self.colorarm.on_to_position(SpeedDPS(600), 0)
def colorarm_remove_halfway(self):
log.info("colorarm_remove_halfway()")
self.colorarm.on_to_position(SpeedDPS(600), -400)
def scan_face(self, face_number):
log.info("scan_face() %d/6" % face_number)
if self.shutdown:
return
if self.flipper.position > 35:
self.flipper_away(100)
self.colorarm_middle()
self.colors[int(MindCuber.scan_order[self.k])] = self.color_sensor.rgb
self.k += 1
i = 1
target_pos = 115
self.colorarm_corner(i)
# The gear ratio is 3:1 so 1080 is one full rotation
self.turntable.reset()
self.turntable.on_to_position(SpeedDPS(MindCuber.rotate_speed),
1080,
block=False)
self.turntable.wait_until('running')
while True:
# 135 is 1/8 of full rotation
if self.turntable.position >= target_pos:
current_color = self.color_sensor.rgb
self.colors[int(MindCuber.scan_order[self.k])] = current_color
i += 1
self.k += 1
if i == 9:
# Last face, move the color arm all the way out of the way
if face_number == 6:
self.colorarm_remove()
# Move the color arm far enough away so that the flipper
# arm doesn't hit it
else:
self.colorarm_remove_halfway()
break
elif i % 2:
self.colorarm_corner(i)
if i == 1:
target_pos = 115
elif i == 3:
target_pos = 380
else:
target_pos = i * 135
else:
self.colorarm_edge(i)
if i == 2:
target_pos = 220
elif i == 8:
target_pos = 1060
else:
target_pos = i * 135
if self.shutdown:
return
if i < 9:
raise ScanError('i is %d..should be 9' % i)
self.turntable.wait_until_not_moving()
self.turntable.off()
self.turntable.reset()
log.info("\n")
def scan(self):
log.info("scan()")
self.colors = {}
self.k = 0
self.scan_face(1)
self.flip()
self.scan_face(2)
self.flip()
self.scan_face(3)
self.rotate_cube(-1, 1)
self.flip()
self.scan_face(4)
self.rotate_cube(1, 1)
self.flip()
self.scan_face(5)
self.flip()
self.scan_face(6)
if self.shutdown:
return
log.info("RGB json:\n%s\n" % json.dumps(self.colors))
self.rgb_solver = RubiksColorSolverGeneric(3)
self.rgb_solver.enter_scan_data(self.colors)
self.rgb_solver.crunch_colors()
self.cube_kociemba = self.rgb_solver.cube_for_kociemba_strict()
log.info("Final Colors (kociemba): %s" % ''.join(self.cube_kociemba))
# This is only used if you want to rotate the cube so U is on top, F is
# in the front, etc. You would do this if you were troubleshooting color
# detection and you want to pause to compare the color pattern on the
# cube vs. what we think the color pattern is.
'''
log.info("Position the cube so that U is on top, F is in the front, etc...to make debugging easier")
self.rotate_cube(-1, 1)
self.flip()
self.flipper_away()
self.rotate_cube(1, 1)
input('Paused')
'''
def move(self, face_down):
log.info("move() face_down %s" % face_down)
position = self.state.index(face_down)
actions = {
0: ["flip", "flip"],
1: [],
2: ["rotate_cube_2", "flip"],
3: ["rotate_cube_1", "flip"],
4: ["flip"],
5: ["rotate_cube_3", "flip"]
}.get(position, None)
for a in actions:
if self.shutdown:
break
getattr(self, a)()
def run_kociemba_actions(self, actions):
log.info('Action (kociemba): %s' % ' '.join(actions))
total_actions = len(actions)
for (i, a) in enumerate(actions):
if self.shutdown:
break
if a.endswith("'"):
face_down = list(a)[0]
rotation_dir = 1
elif a.endswith("2"):
face_down = list(a)[0]
rotation_dir = 2
else:
face_down = a
rotation_dir = 3
log.info("Move %d/%d: %s%s (a %s)" %
(i, total_actions, face_down, rotation_dir, pformat(a)))
self.move(face_down)
if rotation_dir == 1:
self.rotate_cube_blocked_1()
elif rotation_dir == 2:
self.rotate_cube_blocked_2()
elif rotation_dir == 3:
self.rotate_cube_blocked_3()
log.info("\n")
def resolve(self):
if self.shutdown:
return
cmd = ['kociemba', ''.join(map(str, self.cube_kociemba))]
output = check_output(cmd).decode('ascii')
if 'ERROR' in output:
msg = "'%s' returned the following error\n%s\n" % (' '.join(cmd),
output)
log.error(msg)
print(msg)
sys.exit(1)
actions = output.strip().split()
self.run_kociemba_actions(actions)
self.cube_done()
def cube_done(self):
self.flipper_away()
def wait_for_cube_insert(self):
rubiks_present = 0
rubiks_present_target = 10
log.info('wait for cube...to be inserted')
while True:
if self.shutdown:
break
dist = self.infrared_sensor.proximity
# It is odd but sometimes when the cube is inserted
# the IR sensor returns a value of 100...most of the
# time it is just a value less than 50
if dist < 50 or dist == 100:
rubiks_present += 1
log.info("wait for cube...distance %d, present for %d/%d" %
(dist, rubiks_present, rubiks_present_target))
else:
if rubiks_present:
log.info('wait for cube...cube removed (%d)' % dist)
rubiks_present = 0
if rubiks_present >= rubiks_present_target:
log.info('wait for cube...cube found and stable')
break
time.sleep(0.1)
class ALBERT(object):
def __init__(self):
# ev3dev initialization
self.leds = Leds()
self.sound = Sound()
self.arm_base = LargeMotor(OUTPUT_D)
self.arm_shoulder = LargeMotor(OUTPUT_C)
self.arm_wrist = LargeMotor(OUTPUT_B)
self.arm_gripper = MediumMotor(OUTPUT_A)
self.station = Workstation()
self.color_sensor = ColorSensor(INPUT_1)
self.find_indicator()
self.rotate_base(STATION_COLOR)
self.reset_all_motors()
def find_indicator(self):
''' Search for a valid color indicator. '''
if self.color_sensor.color in VALID_COLORS:
return
self.arm_base.on(10)
while self.color_sensor.color not in VALID_COLORS:
pass
self.arm_base.stop()
def reset_all_motors(self):
''' Reset all motor tach counts. '''
self.arm_base.reset()
self.arm_shoulder.reset()
self.arm_wrist.reset()
self.arm_gripper.reset()
def rotate_base(self, color):
'''
Rotate from one color indicator to another.
Color order is:
YELLOW <--> BLUE <--> RED
STORE <--> STATION <--> STERILE
'''
current_color = self.color_sensor.color
if current_color == color:
return
direction = 1
if (current_color == STATION_COLOR
and color == STERILE_COLOR) or current_color == STORE_COLOR:
direction = -1
self.arm_base.on(SPEEDS[0] * direction, block=False)
while self.color_sensor.color != color:
pass
self.arm_base.stop()
self.arm_base.on_for_rotations(SPEEDS[0], direction * STRIPE_BIAS)
def make_plate(self):
''' Sequence to make a plate. '''
self.get_plate()
self.lift_lid()
self.swab_plate()
self.lower_lid()
self.store_plate()
def check_plate(self):
''' Sequence to check plate. '''
self.get_plate(from_storage=True, upside_down=True)
self.move_to_keypoint(KP_UP_HORZ)
refl = self.station.check_status()
self.move_to_keypoint(KP_DOWN_HORZ)
self.store_plate(is_upside_down=True)
return refl
def get_plate(self, from_storage=False, upside_down=False):
'''
Sequence to get a plate and place it in the workstation.
Post-conditions
Gripper: WIDE
Arm: DOWN
Base: STATION
'''
src = STORE_COLOR if from_storage else STERILE_COLOR
self.move_to_keypoint(KP_UP_HORZ)
self.rotate_base(src)
self.set_gripper(GRIP_NARROW)
self.move_to_keypoint(KP_DOWN_HORZ)
self.set_gripper(GRIP_CLOSED)
self.move_to_keypoint(KP_UP_HORZ)
self.rotate_base(STATION_COLOR)
dest_up = KP_UP_VERT_INVERT if upside_down else KP_UP_VERT
dest_down = KP_DOWN_VERT_INVERT if upside_down else KP_DOWN_VERT
self.move_to_keypoint(dest_up)
self.move_to_keypoint(dest_down)
self.set_gripper(GRIP_WIDE)
self.move_to_keypoint(KP_DOWN_HORZ)
def lift_lid(self):
'''
Lift the dish lid.
Pre-condition
Gripper: WIDE
Arm: DOWN
Base: STATION
Post-condition
Gripper: CLOSED
Arm: UP
'''
self.move_to_keypoint(KP_DOWN_HORZ_LID)
self.set_gripper(GRIP_CLOSED)
self.move_to_keypoint(KP_UP_HORZ)
def lower_lid(self):
'''
Lower the dish lid.
Pre-condition
Gripper: CLOSED
Arm: UP
Base: STATION
Post-condition
Gripper: WIDE
Arm: DOWN
'''
self.move_to_keypoint(KP_DOWN_HORZ_LID)
self.set_gripper(GRIP_WIDE)
def swab_plate(self):
''' Call the Workstation swab routine. '''
self.station.swab()
def store_plate(self, is_upside_down=False):
''' Sequence to store a plate. '''
src_down = KP_DOWN_VERT_INVERT if is_upside_down else KP_DOWN_VERT
self.move_to_keypoint(src_down)
self.set_gripper(GRIP_CLOSED)
self.move_to_keypoint(KP_UP_HORZ)
self.rotate_base(STORE_COLOR)
self.move_to_keypoint(KP_DOWN_HORZ)
self.set_gripper(GRIP_NARROW)
self.move_to_keypoint(KP_UP_VERT)
self.rotate_base(STATION_COLOR)
self.set_gripper(GRIP_CLOSED)
def move_to_keypoint(self, keypoint):
''' Move the should/wrist to a keypoint.'''
# keypoint is [shoulder, wrist] with unit of rotations
self.arm_shoulder.on_to_position(
SPEEDS[1], keypoint[0] * self.arm_shoulder.count_per_rot)
self.arm_wrist.on_to_position(
SPEEDS[2], keypoint[1] * self.arm_wrist.count_per_rot)
# pause to let things settle
time.sleep(MOVE_PAUSE)
def set_gripper(self, position):
''' Set the gripper position. '''
self.arm_gripper.on_to_position(
25, self.arm_gripper.count_per_rot * position)
time.sleep(MOVE_PAUSE)
class AthenaRobot(object):
# constructors for the robot with default parameters of wheel radius and ports
def __init__(self,
wheelRadiusCm=4,
leftLargeMotorPort=OUTPUT_B,
rightLargeMotorPort=OUTPUT_C,
leftMediumMotorPort=OUTPUT_A,
rightMediumMotorPort=OUTPUT_D,
leftSensorPort=INPUT_1,
rightSensorPort=INPUT_4,
ultraSonicSensorPort=INPUT_2):
#self is the current object, everything below for self are member variables
self.wheelRadiusCm = wheelRadiusCm
self.wheelCircumferenceCm = 2 * math.pi * wheelRadiusCm
self.leftLargeMotor = LargeMotor(leftLargeMotorPort)
self.rightLargeMotor = LargeMotor(rightLargeMotorPort)
self.leftMediumMotor = MediumMotor(leftMediumMotorPort)
self.rightMediumMotor = MediumMotor(rightMediumMotorPort)
self.leftSensor = ColorSensor(leftSensorPort)
self.rightSensor = ColorSensor(rightSensorPort)
# run a distance in centimeters at speed of centimeters per second
def run(self, distanceCm, speedCmPerSecond, brake=True, block=True):
if speedCmPerSecond < 0:
raise Exception('speed cannot be negative')
# Calculate degrees of distances and SpeedDegreePerSecond
degreesToRun = distanceCm / self.wheelCircumferenceCm * 360
speedDegreePerSecond = speedCmPerSecond / self.wheelCircumferenceCm * 360
print("Run - Degree: {0:.3f} Speed:{1:.3f} MaxSpeed {2}".format(
degreesToRun, speedDegreePerSecond, self.leftLargeMotor.max_speed),
file=sys.stderr)
# run motors based on the calculated results
self.leftLargeMotor.on_for_degrees(
SpeedDPS(speedDegreePerSecond) * (-1), degreesToRun, brake, False)
self.rightLargeMotor.on_for_degrees(
SpeedDPS(speedDegreePerSecond) * (-1), degreesToRun, brake, block)
# turn a angle in degrees, positive means turn right and negative means turn left.
def turn(self, degree, speed=10, brake=True, block=True):
# 1.9 is a scale factor from experiments
degreesToRun = degree * 1.275
# Turn at the speed
self.leftLargeMotor.on_for_degrees(-speed, degreesToRun, brake, False)
self.rightLargeMotor.on_for_degrees(speed, degreesToRun, brake, block)
def turnRight(self, degree, speed=10, brake=True, block=True):
self.turn(degree, speed, brake, block)
def turnLeft(self, degree, speed=10, brake=True, block=True):
self.turn(-degree, speed, brake, block)
def turnOnRightWheel(self, degree, speed=10, brake=True, block=True):
degreesToRun = degree * 2.7
self.rightLargeMotor.on_for_degrees(speed, degreesToRun, brake, block)
def turnRightOnRightWheel(self, degree, speed=10, brake=True, block=True):
self.turnOnRightWheel(degree, speed, brake, block)
def turnLeftOnRightWheel(self, degree, speed=10, brake=True, block=True):
self.turnOnRightWheel(-degree, speed, brake, block)
def turnOnLeftWheel(self, degree, speed=10, brake=True, block=True):
degreesToRun = degree * 2.7
self.leftLargeMotor.on_for_degrees(-speed, degreesToRun, brake, block)
def turnRightOnLeftWheel(self, degree, speed=10, brake=True, block=True):
self.turnOnLeftWheel(degree, speed, brake, block)
def turnLeftOnLeftWheel(self, degree, speed=10, brake=True, block=True):
self.turnOnLeftWheel(-degree, speed, brake, block)
#Medium Motor Movement
def moveMediumMotor(self, isLeft, speed, degrees, brake=True, block=True):
#sees which motor is running
if isLeft == False:
self.rightMediumMotor.on_for_degrees(speed, degrees, brake, block)
else:
self.leftMediumMotor.on_for_degrees(speed, degrees, brake, block)
# Following a line with one sensor
def lineFollow(self,
whiteThreshold=98,
blackThreshold=15,
scale=0.2,
useLeftSensor=True,
useLeftEdge=True,
runDistanceCM=300):
self.leftLargeMotor.reset()
self.rightLargeMotor.reset()
# Allow an attached backsensor. Ixf useBackSensor, defining back sensor and revert useLeftEdge since motor is actually going backward
initialPos = self.leftLargeMotor.position # remember initial position
loop = True
while loop:
# use left or right sensor based on passed in useLeftSensor
reflect = self.leftSensor.reflected_light_intensity if useLeftSensor == True else self.rightSensor.reflected_light_intensity
# Allow an attached backsensor. If useBackSensor, use reflected_light_intensity of that sensor
leftPower = abs(reflect - blackThreshold) * scale
rightPower = abs(whiteThreshold - reflect) * scale
# if we follow the right edge, need to swap left and right
if useLeftEdge == False:
oldLeft = leftPower
leftPower = rightPower
rightPower = oldLeft
self.leftLargeMotor.on(-leftPower)
self.rightLargeMotor.on(-rightPower)
# Calculate the distance run in CM
distanceRanInCM = abs((self.leftLargeMotor.position - initialPos) *
(self.wheelCircumferenceCm /
self.leftLargeMotor.count_per_rot))
# Printing the reflected light intensity with the powers of the two motors
print(
"LineFollow - reflect: {0:3d} leftPower: {1:3f} rightPower: {2:3f} lMotorPos: {3:3d} distanceRanInCM {4:3f}"
.format(reflect, leftPower, rightPower,
self.leftLargeMotor.position, distanceRanInCM),
file=sys.stderr)
if distanceRanInCM >= runDistanceCM:
loop = False
# Stopping the motor once the loop is over
self.leftLargeMotor.off()
self.rightLargeMotor.off()
# run until both conditions are met
def onUntilTwoConditions(self,
leftCondition,
rightCondition,
caller,
speed=5,
consecutiveHit=5,
sleepTime=0.01):
# Start motor at passed speonUntilTwoConditionsed.
self.leftLargeMotor.on(-speed)
self.rightLargeMotor.on(-speed)
condLeftCounter = 0
condRightCounter = 0
condLeftMet = False
condRightMet = False
while (not condLeftMet or not condRightMet):
# check left condition
if (leftCondition()):
condLeftCounter += 1
else:
condLeftCounter = 0
# reset to zero
if (condLeftCounter >= consecutiveHit):
if (condRightMet):
sleep(.1)
self.leftLargeMotor.off()
condLeftMet = True
# check right condition
if (rightCondition()):
condRightCounter += 1
else:
condRightCounter = 0
# reset to zero
if (condRightCounter >= consecutiveHit):
if (condLeftMet):
sleep(.1)
self.rightLargeMotor.off()
condRightMet = True
print(
"{4}-onUntilTwoConditions: left_reflected: {0:3d}, right_reflected: {1:3d}, leftHit: {2:3d}, rightHit: {3:3d}"
.format(self.leftSensor.reflected_light_intensity,
self.rightSensor.reflected_light_intensity,
condLeftCounter, condRightCounter, caller),
file=sys.stderr)
sleep(sleepTime)
self.leftLargeMotor.off()
self.rightLargeMotor.off()
def onUntilWhiteLine(self,
consecutiveHit=5,
speed=5,
sleepTime=0.01,
white_threshold=85):
self.onUntilTwoConditions(
lambda: self.leftSensor.reflected_light_intensity >
white_threshold, lambda: self.rightSensor.reflected_light_intensity
> white_threshold, "onUntilWhiteLine", speed, consecutiveHit,
sleepTime)
def onUntilBlackLine(self,
consecutiveHit=5,
speed=5,
sleepTime=0.01,
black_threshold=30):
self.onUntilTwoConditions(
lambda: self.leftSensor.reflected_light_intensity <
black_threshold, lambda: self.rightSensor.reflected_light_intensity
< black_threshold, "onUntilBlackLine", speed, consecutiveHit,
sleepTime)
# run until find a game line
def onUntilGameLine(self,
consecutiveHit=5,
speed=5,
sleepTime=0.01,
white_threshold=85,
black_threshold=30):
self.onUntilWhiteLine(consecutiveHit, speed, sleepTime,
white_threshold)
self.onUntilBlackLine(consecutiveHit, speed, sleepTime,
black_threshold)
# run until condition is met
def onUntilCondition(self,
condition,
caller,
leftSpeed=5,
rightSpeed=5,
consecutiveHit=5,
sleepTime=0.01,
revert=False):
# Start motor at passed speonUntilTwoConditionsed.
self.leftLargeMotor.on(-leftSpeed if revert == False else leftSpeed)
self.rightLargeMotor.on(-rightSpeed if revert == False else rightSpeed)
counter = 0
condMet = False
while (not condMet):
# check condition
if (condition()):
counter += 1
else:
counter = 0
# reset to zero
if (counter >= consecutiveHit):
self.leftLargeMotor.off()
self.rightLargeMotor.off()
condMet = True
print(
"{4}-onUntilCondition: ColorSensor(left_reflected: {0:3d}, right_reflected: {1:3d}, hit: {2:3d}), UltraSonicSensor(distance_centimeters: {3:3f})"
.format(self.leftSensor.reflected_light_intensity,
self.rightSensor.reflected_light_intensity, counter,
self.ultraSonicSensor.distance_centimeters, caller),
file=sys.stderr)
sleep(sleepTime)
self.leftLargeMotor.off()
self.rightLargeMotor.off()
def onUntilLeftBlack(self,
speed=5,
consecutiveHit=5,
sleepTime=0.01,
black_threshold=30):
self.onUntilCondition(
lambda: self.leftSensor.reflected_light_intensity <
black_threshold, "onUntilLeftBlack", speed, speed, consecutiveHit,
sleepTime)
def onUntilLeftWhite(self,
speed=5,
consecutiveHit=5,
sleepTime=0.01,
white_threshold=85):
self.onUntilCondition(
lambda: self.leftSensor.reflected_light_intensity >
white_threshold, "onUntilLeftWhite", speed, speed, consecutiveHit,
sleepTime)
def onUntilRightBlack(self,
speed=5,
consecutiveHit=5,
sleepTime=0.01,
black_threshold=30):
self.onUntilCondition(
lambda: self.rightSensor.reflected_light_intensity <
black_threshold, "onUntilRightBlack", speed, speed, consecutiveHit,
sleepTime)
def onUntilRightWhite(self,
speed=5,
consecutiveHit=5,
sleepTime=0.01,
white_threshold=85):
self.onUntilCondition(
lambda: self.rightSensor.reflected_light_intensity >
white_threshold, "onUntilRightWhite", speed, speed, consecutiveHit,
sleepTime)
def turnUntilLeftBlack(self,
turnLeft,
speed,
consecutiveHit=2,
black_threshold=30):
self.onUntilCondition(
lambda: self.leftSensor.reflected_light_intensity <
black_threshold, "turnUntilLeftBlack",
0 if turnLeft == True else speed, speed if turnLeft == True else 0,
consecutiveHit)
def turnUntilLeftWhite(self,
turnLeft,
speed,
consecutiveHit=2,
white_threshold=85):
self.onUntilCondition(
lambda: self.leftSensor.reflected_light_intensity >
white_threshold, "turnUntilLeftWhite",
0 if turnLeft == True else speed, speed if turnLeft == True else 0,
consecutiveHit)
def turnUntilRightBlack(self,
turnLeft,
speed,
consecutiveHit=2,
black_threshold=30):
self.onUntilCondition(
lambda: self.rightSensor.reflected_light_intensity <
black_threshold, "turnUntilRightBlack",
0 if turnLeft == True else speed, speed if turnLeft == True else 0,
consecutiveHit)
def turnUntilRightWhite(self,
turnLeft,
speed,
consecutiveHit=2,
white_threshold=85):
self.onUntilCondition(
lambda: self.rightSensor.reflected_light_intensity >
white_threshold, "turnUntilRightWhite",
0 if turnLeft == True else speed, speed if turnLeft == True else 0,
consecutiveHit)
# Go until sensor reading has a specified offset or reach to the threshhold
def onUntilRightDarkerBy(self,
difference,
black_threshold=20,
speed=10,
consecutiveHit=2):
originalValue = self.rightSensor.reflected_light_intensity
print("onUntilRightDarkerBy - originalValue: {0:3d}, diff: {1:3d}".
format(originalValue, difference),
file=sys.stderr)
self.onUntilCondition(
lambda: self.rightSensor.reflected_light_intensity - originalValue
< -difference or self.rightSensor.reflected_light_intensity <
black_threshold,
"onUntilRightSensorDiff",
consecutiveHit=consecutiveHit)
def onUntilRightLighterBy(self,
difference,
white_threshold=80,
speed=10,
consecutiveHit=2):
originalValue = self.rightSensor.reflected_light_intensity
print("onUntilRightLighterBy - originalValue: {0:3d}, diff: {1:3d}".
format(originalValue, difference),
file=sys.stderr)
self.onUntilCondition(
lambda: self.rightSensor.reflected_light_intensity - originalValue
> difference or self.rightSensor.reflected_light_intensity >
white_threshold,
"onUntilRightSensorDiff",
consecutiveHit=consecutiveHit)
def onUntilLeftDarkerBy(self,
difference,
black_threshold=20,
speed=10,
consecutiveHit=2):
originalValue = self.leftSensor.reflected_light_intensity
print(
"onUntilLeftDarkerBy - originalValue: {0:3d}, diff: {1:3d}".format(
originalValue, difference),
file=sys.stderr)
self.onUntilCondition(lambda: self.leftSensor.reflected_light_intensity
- originalValue < -difference or self.leftSensor.
reflected_light_intensity < black_threshold,
"onUntilLeftSensorDiff",
consecutiveHit=consecutiveHit)
def onUntilLeftLighterBy(self,
difference,
white_threshold=80,
speed=10,
consecutiveHit=2):
originalValue = self.leftSensor.reflected_light_intensity
print("onUntilLeftLighterBy - originalValue: {0:3d}, diff: {1:3d}".
format(originalValue, difference),
file=sys.stderr)
self.onUntilCondition(lambda: self.leftSensor.reflected_light_intensity
- originalValue > difference or self.leftSensor.
reflected_light_intensity > white_threshold,
"onUntilLeftSensorDiff",
consecutiveHit=consecutiveHit)
#uses Ultrasonic sensor to see wall as going back
def revertSafely(self,
speed=100,
distanceToStop=10,
consecutiveHit=1,
sleepTime=0.01):
self.onUntilCondition(
lambda: self.ultraSonicSensor.distance_centimeters <
distanceToStop, "revertSafely", speed, speed, consecutiveHit,
sleepTime, True)
# Calibrating White for Sensor
def calibrateColorSensor(self, sensorInput):
sensor = ColorSensor(sensorInput)
# Calibration
sensor.calibrate_white()
# Done Signal
sound.beep()
# Calibrating Color for Sensor
def testColorSensor(self,
sensorInput,
sensorPort,
repeatNumber=10,
pauseNumber=0.2,
speed=0):
sensor = ColorSensor(sensorInput)
if (speed > 0):
self.leftLargeMotor.on(speed)
self.rightLargeMotor.on(speed)
times = 0
# For loop
while times != repeatNumber:
# Print
print("testColorSensor- Port: {0:3d}: {1:3d}".format(
sensorPort, sensor.reflected_light_intensity),
file=sys.stderr)
time.sleep(pauseNumber)
times = times + 1
self.leftLargeMotor.off()
self.rightLargeMotor.off()
def resetMediumMotor():
self.moveMediumMotor(isLeft=False, speed=50, degrees=1000)
self.rightMediumMotor.reset()
def testRobot(self):
self.leftLargeMotor.on_for_degrees(20, 180)
sleep(.1)
self.rightLargeMotor.on_for_degrees(20, 180)
sleep(.1)
self.moveMediumMotor(True, 10, 180)
sleep(.1)
self.moveMediumMotor(False, 10, 180)
sleep(.1)
self.run(20, 10)
sleep(.1)
self.run(-20, 10)
sleep(.1)
self.turn(90)
sleep(.1)
self.turn(-90)
sleep(.1)
self.turnOnLeftWheel(90)
sleep(.1)
self.turnOnLeftWheel(-90)
sleep(.1)
self.turnOnRightWheel(90)
sleep(.1)
self.turnOnRightWheel(-90)
sleep(.1)
self.calibrateColorSensor(INPUT_1)
self.calibrateColorSensor(INPUT_4)
self.testColorSensor(INPUT_1, 1)
self.testColorSensor(INPUT_4, 4)
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 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()
screenw = display.xres
screenh = display.yres
# hostMACAddress = '00:17:E9:B2:8A:AF' # The MAC address of a Bluetooth adapter on the server. The server might have multiple Bluetooth adapters.
# Fetch BT MAC address automatically
cmd = "hciconfig"
device_id = "hci0"
sp_result = subprocess.run(cmd, stdout=subprocess.PIPE, universal_newlines=True)
hostMACAddress = sp_result.stdout.split("{}:".format(device_id))[1].split("BD Address: ")[1].split(" ")[0].strip()
debug_print (hostMACAddress)
print (hostMACAddress)
# reset the kick motor to a known good position
kickMotor.on_for_seconds(speed=-10, seconds=0.5)
kickMotor.on_for_seconds(speed=10, seconds=2, brake=False)
kickMotor.reset()
kicking = False
kick_power = 0
max_kick = 1000
port = 3 # port number is arbitrary, but must match between server and client
backlog = 1
size = 1024
s = bluetooth.BluetoothSocket(bluetooth.RFCOMM)
s.bind((hostMACAddress, port))
s.listen(backlog)
while True:
try:
reset_console()
print (hostMACAddress)
leds.set_color('LEFT', 'AMBER')
from ev3dev2.motor import MediumMotor
from time import sleep
mA = MediumMotor('outA')
mA.reset()
def lift(direction):
if(direction == "up"):
mA.run_to_rel_pos(position_sp=130, speed_sp=300, stop_action="coast")
else:
mA.run_to_rel_pos(position_sp=-130, speed_sp=300)
mA.wait_while('running')