from pybricks.robotics import DriveBase
from pybricks.media.ev3dev import SoundFile, ImageFile
from time import sleep
# This program requires LEGO EV3 MicroPython v2.0 or higher.
# Click "Open user guide" on the EV3 extension tab for more information.
# Create your objects here.
ev3 = EV3Brick()
# Write your program here.
ev3.speaker.beep()
# Initialize the motors.
left_motor = Motor(Port.A)
right_motor = Motor(Port.B)
# Initialize the drive base.
robot = DriveBase(left_motor, right_motor, wheel_diameter=55.5, axle_track=104)
ir_sensor = InfraredSensor(Port.S4)
ir_sensor.beacon(1)
while True:
buttons = ir_sensor.keypad()
if len(buttons) == 1:
print(buttons[0])
if (buttons[0] == Button.LEFT_UP):
print("###")
robot.straight(-30)
elif (buttons[0] == Button.RIGHT_UP):
pass
elif (buttons[0] == Button.LEFT_DOWN):
pass
elif (buttons[0] == Button.RIGHT_DOWN):
pass
elif len(buttons) == 2:
print(str(buttons[0]) + " and " + str(buttons[1]))
) # left sensor motor is used to measure the sensor angle
# Main loop
loopCount = 0 # we only update some (slow) logic every X logical frames
while (True):
ev3.screen.clear()
DrawText("~~ JL ROBOT " + str(loopCount) + " ~~", 4)
# Outputs
moveDirection = 0 # 0 = stop, 1 = forward, 2 = backward
moveImpulseTime = -1 # milliseconds, -1 = continious
sensorDirection = 0
# Read infrared beacon
if (enableBeaconMoves and loopCount % 100 == 0):
beaconData = ir.beacon(infraredChannel)
print(str(beaconData))
# Back up a bit if controller gets close
# NOTE: The distance takes a long time to update, so we need a hysteresis to make sure we only perform a small impulse rather than backing up for many seconds
beaconDistance = beaconData[0]
if beaconDistance is not None and beaconDistance <= moveBackDistanceThreshold:
if not hasRecentlyFallenBackIR:
moveDirection = 2
moveImpulseTime = 1500
hasRecentlyFallenBackIR = True
else:
hasRecentlyFallenBackIR = False
# Move sensor platform towards beacon platform
beaconAngle = beaconData[1]
straight_speed=300, # milimeters per second
straight_acceleration=300,
turn_rate=90, # degrees per second
turn_acceleration=90)
MEDIUM_MOTOR = Motor(port=Port.A, positive_direction=Direction.CLOCKWISE)
IR_SENSOR = InfraredSensor(port=Port.S4)
while True:
BRICK.light.on(color=Color.ORANGE)
if Button.BEACON in IR_SENSOR.buttons(channel=1):
# FIXME: make it work
distance, heading = IR_SENSOR.beacon(channel=1)
heading_difference = heading - (-3)
proximity_difference = distance - 70
if (heading_difference == 0) and (proximity_difference == 0):
DRIVE_BASE.stop()
BRICK.light.on(color=Color.RED)
MEDIUM_MOTOR.run_angle(
speed=1000, # degrees per second
rotation_angle=6 * 360, # degrees
then=Stop.HOLD,
wait=True)
BRICK.speaker.play_file(file=SoundFile.LAUGHING_2)
#
# Start
#
brick.sound.beep(2000)
brick.sound.beep(1000)
brick.sound.beep(5000)
useful_buttons = [
Button.LEFT_UP,
Button.LEFT_DOWN,
Button.RIGHT_UP,
Button.RIGHT_DOWN,
]
while not touch_sensor.pressed():
(dist, angle) = ir.beacon(1)
buttons = ir.buttons(1)
print(dist, angle, buttons)
# if angle is not None and dist > 15:
# tracks.drive(2 * dist, 2 * angle)
if useful_buttons in buttons:
if Button.LEFT_UP in buttons and Button.RIGHT_UP in buttons:
print("forward")
tracks.drive(200, 0)
elif Button.LEFT_UP in buttons:
print("left")
tracks.drive(200, -30)
elif Button.RIGHT_UP in buttons:
print("right")
tracks.drive(200, 30)
elif Button.LEFT_DOWN in buttons and Button.RIGHT_DOWN in buttons:
MEDIUM_MOTOR.run_time(speed=500, time=1000, then=Stop.HOLD, wait=True)
MEDIUM_MOTOR.run_time(speed=-500, time=0.3 * 1000, then=Stop.HOLD, wait=True)
STING_MOTOR.run_time(speed=400, time=1000, then=Stop.HOLD, wait=True)
GO_MOTOR.run(speed=-500)
BRICK.speaker.play_file(file=SoundFile.ERROR_ALARM)
ir_beacon_distance = ir_beacon_angle = None
while (ir_beacon_distance is None) or (ir_beacon_angle is
None) or (ir_beacon_distance >= 30):
ir_beacon_distance, ir_beacon_angle = IR_SENSOR.beacon(channel=1)
while ir_beacon_angle <= 5:
ir_beacon_distance, ir_beacon_angle = IR_SENSOR.beacon(channel=1)
GO_MOTOR.run(speed=-200)
while ir_beacon_angle >= 3:
ir_beacon_distance, ir_beacon_angle = IR_SENSOR.beacon(channel=1)
GO_MOTOR.stop()
BRICK.speaker.play_file(file=SoundFile.ERROR_ALARM)
for i in range(3):
GO_MOTOR.run_angle(speed=-1000,
ev3.speaker.say('come to me')
dist = 100
while dist > 30:
wait(10)
dist = ir.distance()
ev3.screen.print('Distance: ', dist, 'mm')
ev3.speaker.say('come to me on channel 1')
dist = 100
angle = 0
while dist > 10:
wait(10)
dist, angle = ir.beacon(1)
ev3.screen.print('Distance: ', dist*2, 'mm')
ev3.screen.print('Angle: ', angle, 'deg')
ev3.speaker.say('last try, use channel 1')
while True:
buttons = ir.keypad()
wait(10)
if Button.LEFT_UP in buttons and Button.RIGHT_UP in buttons and Button.RIGHT_DOWN in buttons:
ev3.speaker.play_file(Sound.GOODBYE)
break
elif Button.LEFT_UP in buttons and Button.RIGHT_UP in buttons:
ev3.speaker.play_file(Sound.UP)
elif Button.LEFT_DOWN in buttons and Button.RIGHT_DOWN in buttons: