screen = Screen()
screen.hide_cursor()
w, h = screen.shape
for image_file in ('tux.png', 'pobot.png', 'smiley.png'):
decal = Image.open(os.path.join(os.path.dirname(__file__), 'img', image_file))
iw, ih = decal.size
screen.draw.rectangle(
((0, 0), (w-1, h-1)),
outline='black'
)
for i, dim in enumerate(zip('wh', (w, h))):
label, value = dim
s = "%s=%d" % (label, value)
tw, th = screen.draw.textsize(s)
screen.draw.text((w - tw - 1, 2 + i * th), s)
screen.draw.arc((5, 5, 25, 25), 0, 360)
screen.draw.arc((w - 25, h - 25, w - 5, h - 5), 0, 360)
screen.img.paste(decal, ((w - iw)/2, (h - ih)/2, (w + iw)/2, (h + ih)/2), decal)
screen.update()
time.sleep(5)
screen.clear()
class Grabber(object):
WHEELS_DIST = 140 # mm
WHEEL_DIAMETER = 43.2 # mm
EXPLORATION_RANGE = 300 # mm
DEPOSIT_DIST = 250 # mm
def __init__(self):
self._motor_left = ev3.LargeMotor(port='outB')
self._motor_right = ev3.LargeMotor(port='outC')
self._motors = (self._motor_left, self._motor_right)
self._gripper = Gripper(ev3.MediumMotor(port='outA'))
self._color_sensor = ev3.ColorSensor(port='in1')
self._start_btn = ev3.TouchSensor(port='in2')
self._buttons = ev3.Buttons()
self._buttons.on_back = self._back_button_pressed
self._screen = Screen()
self._screen.hide_cursor()
self._done = False
self._dist_per_pulse = self.WHEEL_DIAMETER * math.pi / self._motors[0].count_per_rot
self._spin_per_pulse = math.degrees(self._dist_per_pulse / self.WHEELS_DIST * 2)
self._images = dict()
def _back_button_pressed(self, state):
self._done = True
def reset(self):
for m in self._motors:
m.reset()
m.duty_cycle_sp = 100
m.stop_command = ev3.LargeMotor.STOP_COMMAND_HOLD
m.ramp_up_sp = 500
m.ramp_down_sp = 500
self._color_sensor.mode = ev3.ColorSensor.MODE_COL_COLOR
ev3.Leds.all_off()
self._done = False
def display_image(self, name, centered=True):
# cache images to avoid re-reading them for each request
# (we use a lazy loading method to avoid too long program
# start time if loading all of them on init)
try:
img = self._images[name]
except KeyError:
self._images[name] = img = Image.open(os.path.join(os.path.dirname(__file__), 'img', name + '.png'))
self._screen.clear()
if centered:
xy = tuple((d1 - d0) / 2 for d1, d0 in zip(self._screen.shape, img.size))
else:
xy = (0, 0)
self._screen.img.paste(img, xy)
self._screen.update()
def run(self):
self.reset()
ev3.Leds.heartbeat(red=1, green=1)
self.display_image("smiley-o")
ev3.Sound.speak("I am calibrating my gripper")
self.display_image("smiley-busy")
self._gripper.calibrate()
self._gripper.open()
ev3.Sound.speak('I am ready').wait()
self._buttons.start_scanner()
self._done = False
try:
while not self._done:
self.display_image("smiley-o")
LedFeedback.k2000(red=1, green=1)
ev3.Sound.speak('Press touch sensor to start').wait()
self.display_image("smiley-asleep")
while not (self._start_btn.is_pressed or self._done):
time.sleep(0.1)
while self._start_btn.is_pressed and not self._done:
time.sleep(0.1)
if not self._done:
self.display_image("smiley-waiting")
LedFeedback.searching()
self.grab_a_brick()
finally:
self._buttons.stop_scanner()
ev3.Leds.red_on()
self.display_image('terminator', centered=False)
ev3.Sound.speak("I'll be back").wait()
def grab_a_brick(self):
# reset encoders
for m in self._motors:
m.position = 0
brick_found = self.drive_until_brick_found(max_dist=self.EXPLORATION_RANGE)
good_one = None # we don't need to set it here, but it's cleaner
if brick_found:
brick_color = self.analyze_brick()
try:
self.display_image("smiley-happy")
msg = "this is a %s brick" % {
ev3.ColorSensor.COLOR_GREEN: 'green',
ev3.ColorSensor.COLOR_RED: 'red',
}[brick_color]
good_one = True
except KeyError:
self.display_image("smiley-wtf")
msg = "I do not know this color"
good_one = False
ev3.Sound.speak(msg).wait()
LedFeedback.brick_color(brick_color)
else:
self.display_image("smiley-sad")
ev3.Sound.speak("There is no brick").wait()
brick_color = None # we don't need to set it, but it's cleaner
ev3.Leds.all_off()
# go back home first in any case
dist = self._motor_left.position * self._dist_per_pulse
backing_sound = ev3.Sound.play(os.path.join(_HERE, 'snd', 'backing_alert.rsf'))
self.drive(-dist)
if brick_found:
if good_one:
turn_direction = {
ev3.ColorSensor.COLOR_GREEN: -1,
ev3.ColorSensor.COLOR_RED: 1,
}[brick_color]
self.spin(turn_direction * 90)
self.drive(self.DEPOSIT_DIST)
self._gripper.open()
self.drive(-self.DEPOSIT_DIST)
self.spin(-turn_direction * 90)
else:
backing_sound.wait()
self._gripper.open()
self.display_image("smiley-o")
ev3.Sound.speak("Please take the brick").wait()
else:
# nothing special to do here
pass
backing_sound.wait()
ev3.Leds.all_off()
def drive_for_ever(self, power_pct=100):
for m in self._motors:
m.run_forever(
duty_cycle_sp=power_pct
)
def drive(self, dist_mm, power_pct=100):
""" Travels straight by a given distance.
:param dist_mm: the distance of the travel
:param power_pct: percent of power
"""
pulses = dist_mm / self._dist_per_pulse
for m in self._motors:
m.position_sp = pulses
m.duty_cycle_sp = power_pct
self.run_and_wait_for_completion(absolute=False)
def run_and_wait_for_completion(self, absolute=True):
for m in self._motors:
m.command = m.COMMAND_RUN_TO_ABS_POS if absolute else m.COMMAND_RUN_TO_REL_POS
while any((m.state and 'holding' not in m.state for m in self._motors)):
time.sleep(0.1)
def stop(self, brake=True):
for m in self._motors:
m.stop(stop_command='brake' if brake else 'coast')
def drive_until_brick_found(self, max_dist=300, power_pct=100):
m = self._motors[0]
limit = m.position + max_dist / self._dist_per_pulse
self.drive_for_ever(power_pct=power_pct)
while m.position <= limit and not self._color_sensor.value():
time.sleep(0.1)
self.stop()
return self._color_sensor.value()
def spin(self, degrees, power_pct=100):
""" Spins by a given amount of degrees.
:param degrees: spin degrees (>0 = CCW)
:param power_pct: percent of power
"""
pulses = degrees / self._spin_per_pulse
for i, m in enumerate(self._motors):
m.duty_cycle_sp = power_pct
m.position_sp = pulses * (1 if i else -1)
self.run_and_wait_for_completion(absolute=False)
def analyze_brick(self):
# move forward a bit gently to place the brick well inside the gripper
self.drive(dist_mm=50, power_pct=30)
self._gripper.close()
# leave some time to the sensor for updating its reading accurately
time.sleep(0.2)
return self._color_sensor.value()