Created
October 17, 2021 15:07
-
-
Save creotiv/f8a13c0fb7f1eaa19642ac3ae8a7d9a7 to your computer and use it in GitHub Desktop.
Lego Mindstorm EV3 R2D2
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
#!/usr/bin/env pybricks-micropython | |
""" | |
Example LEGO® MINDSTORMS® EV3 Robot Educator Driving Base Program | |
----------------------------------------------------------------- | |
This program requires LEGO® EV3 MicroPython v2.0. | |
Download: https://education.lego.com/en-us/support/mindstorms-ev3/python-for-ev3 | |
Building instructions can be found at: | |
https://education.lego.com/en-us/support/mindstorms-ev3/building-instructions#robot | |
""" | |
from pybricks.hubs import EV3Brick | |
from pybricks.ev3devices import Motor, InfraredSensor | |
from pybricks.parameters import Port | |
from pybricks.robotics import DriveBase | |
import random | |
import math | |
from pybricks.media.ev3dev import Image, ImageFile, SoundFile | |
# Initialize the EV3 Brick. | |
ev3 = EV3Brick() | |
img = Image(ImageFile.DIZZY) | |
ev3.screen.load_image(img) | |
eyes = InfraredSensor(Port.S4) | |
degree = -3 | |
# Initialize the motors. | |
head = Motor(Port.A) | |
left_motor = Motor(Port.B) | |
right_motor = Motor(Port.C) | |
base_head_angle = head.angle() | |
robot = DriveBase(left_motor, right_motor, wheel_diameter=30, axle_track=139) | |
d = 100 | |
dead_end = False | |
while 1: | |
try: | |
# Go forward 0.3m | |
if d > 5: | |
robot.straight(-300) | |
where_move = 0 # -1 left, 0 straihght, 1 right | |
_max = 0 | |
for i in [0,-1,1]: | |
d = eyes.distance() | |
if d > _max and not (dead_end and i == 0): | |
where_move = i | |
_max = d | |
dead_end = False | |
if i == 0: | |
head.run_target(150, -30*degree) | |
if i == 1: | |
head.run_target(150, 0) | |
if i == -1: | |
head.run_target(150, 30*degree) | |
if _max <= 80: | |
ev3.speaker.play_file(SoundFile.UH_OH) | |
ev3.speaker.play_file(SoundFile.BACKWARDS) | |
robot.straight(400) | |
# rnd = int(round((random.random() - 0.5)/abs(random.random() - 0.5))) | |
# robot.turn(90*rnd) | |
dead_end = True | |
d = 0 | |
continue | |
if where_move == 0: # straight | |
pass | |
if where_move == 1: # right | |
robot.turn(90) | |
if where_move == -1: # left | |
robot.turn(-90) | |
except Exception as e: | |
ev3.speaker.beep() | |
head.run_target(150, 0) | |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment