class R3ptar(EV3Brick):
def __init__(self,
turn_motor_port: Port = Port.A,
move_motor_port: Port = Port.B,
scare_motor_port: Port = Port.D,
touch_sensor_port: Port = Port.S1,
color_sensor_port: Port = Port.S3,
ir_sensor_port: Port = Port.S4,
ir_beacon_channel: int = 1):
self.turn_motor = Motor(port=turn_motor_port,
positive_direction=Direction.CLOCKWISE)
self.move_motor = Motor(port=move_motor_port,
positive_direction=Direction.CLOCKWISE)
self.scare_motor = Motor(port=scare_motor_port,
positive_direction=Direction.CLOCKWISE)
self.touch_sensor = TouchSensor(port=touch_sensor_port)
self.color_sensor = ColorSensor(port=color_sensor_port)
self.ir_sensor = InfraredSensor(port=ir_sensor_port)
self.ir_beacon_channel = ir_beacon_channel
def drive_once_by_ir_beacon(self, speed: float = 1000):
ir_beacons_pressed = set(
self.ir_sensor.buttons(channel=self.ir_beacon_channel))
if ir_beacons_pressed == {Button.LEFT_UP, Button.RIGHT_UP}:
self.move_motor.run(speed=speed)
elif ir_beacons_pressed == {Button.LEFT_DOWN, Button.RIGHT_DOWN}:
self.move_motor.run(speed=-speed)
elif ir_beacons_pressed == {Button.LEFT_UP}:
self.turn_motor.run(speed=-500)
self.move_motor.run(speed=speed)
elif ir_beacons_pressed == {Button.RIGHT_UP}:
self.turn_motor.run(speed=500)
self.move_motor.run(speed=speed)
elif ir_beacons_pressed == {Button.LEFT_DOWN}:
self.turn_motor.run(speed=-500)
self.move_motor.run(speed=-speed)
elif ir_beacons_pressed == {Button.RIGHT_DOWN}:
self.turn_motor.run(speed=500)
self.move_motor.run(speed=-speed)
else:
self.turn_motor.hold()
self.move_motor.stop()
def bite_by_ir_beacon(self, speed: float = 1000):
if Button.BEACON in self.ir_sensor.buttons(
channel=self.ir_beacon_channel):
self.speaker.play_file(file=SoundFile.SNAKE_HISS)
self.scare_motor.run_time(speed=speed,
time=1000,
then=Stop.HOLD,
wait=True)
self.scare_motor.run_time(speed=-300,
time=1000,
then=Stop.COAST,
wait=True)
while Button.BEACON in self.ir_sensor.buttons(
channel=self.ir_beacon_channel):
pass
def run_away_if_chased(self):
if self.color_sensor.reflection() > 30:
self.move_motor.run_time(speed=500,
time=4000,
then=Stop.HOLD,
wait=True)
for i in range(2):
self.turn_motor.run_time(speed=500,
time=1000,
then=Stop.HOLD,
wait=True)
self.turn_motor.run_time(speed=-500,
time=1000,
then=Stop.HOLD,
wait=True)
def bite_if_touched(self):
if self.touch_sensor.pressed():
self.speaker.play_file(file=SoundFile.SNAKE_HISS)
self.scare_motor.run_time(speed=1000,
time=1000,
then=Stop.HOLD,
wait=True)
self.scare_motor.run_time(speed=-300,
time=1000,
then=Stop.COAST,
wait=True)
def main(self, speed: float = 1000):
while True:
self.drive_once_by_ir_beacon(speed=speed)
self.bite_by_ir_beacon(speed=speed)
self.bite_if_touched()
self.run_away_if_chased()
class MbMotor():
"""
Control a motor, besides the fact that you can detect if a motor got stalled
the main reason for this class is to solve a bug for pybricks.ev3devices.Motor. The bug is that when you set the motor
to move in a Direction.COUNTERCLOCKWISE sometimes it failes to detect it.
This class is made on top of pybricks.ev3devices.Motor
Args:
port (Port): The port of the device
clockwise_direction (bool): Sets the defualt movement of the motor clockwise or counterclockwise, True for clockwise else counterclockwise
exit_exec (Function): Function that returns True or False, the motor will stop if returns True
"""
def __init__(self,
port,
clockwise_direction=True,
exit_exec=lambda: False):
self.core = Motor(port)
self.port = port
self.direction = 1 if clockwise_direction else -1
self.exit_exec = exit_exec
def angle(self):
"""
Get the distance the robot has moved in degrees
Returns:
angle (int): The distance the robot has moved in degrees
"""
return self.core.angle() * self.direction
def speed(self):
"""
Get the speed of the motor
Returns:
speed (int): The current speed of the motor
"""
return self.core.speed() * self.direction
def stalled(self, min_speed=0):
if abs(self.speed()) <= abs(min_speed):
return True
return False
def run_angle(self, speed, angle, wait=True, detect_stall=False):
"""
Run the motor to a specific angle
Args:
speed (int): The speed of the motor
angle (int): Degrees to run the motor at
wait (bool): Sets if the robot is going to stop for the motor to complete this method or not
"""
def exec(self, speed, angle):
moved_enough = False
self.reset_angle()
self.run(speed)
while True:
if abs(self.angle()) > 50:
moved_enough = True
if moved_enough and detect_stall:
if self.stalled():
break
if abs(self.angle()) > abs(angle) or self.exit_exec():
break
self.hold()
if wait:
exec(self, speed, angle)
else:
threading.Thread(target=exec, args=[self, speed, angle]).start()
def run_time(self, speed, msec, wait=True):
"""
Run the motor to a amount of time
Args:
speed (int): The speed of the motor
msec (int): Time to move the robot
wait (bool): Sets if the robot is going to stop for the motor to complete this method or not
"""
def exec(self, speed, msec):
self.reset_angle()
self.run(speed)
s = time()
while True:
if round(time() - s,
2) * 1000 >= abs(msec) or self.exit_exec():
break
self.hold()
if wait:
exec(self, speed, msec)
else:
threading.Thread(target=exec, args=[self, speed, msec]).start()
def run(self, speed):
"""
Run the motor to a constant speed
Args:
speed (int): Speed to run at
Note:
speed parameter should be between -800 and 800
"""
self.core.run(speed * self.direction)
def dc(self, speed):
"""
Run the motor to a constant speed
Args:
speed (int): Speed to run at
Note:
speed parameter should be between -100 and 100
"""
self.core.dc(speed * self.direction)
def hold(self):
"""
Stop the motor and hold its position
"""
self.core.hold()
def brake(self):
"""
Passively stop the motor
"""
self.core.brake()
def stop(self):
"""
No current is being aplied to the robot, so its gonna stop due to friction
"""
self.core.stop()
def reset_angle(self, angle=0):
"""
Set the motor angle
Args:
angle (int): Angle to set the motor at
"""
self.core.reset_angle(angle)
def is_stalled(self, min_speed=0):
"""
Check if the motor got stalled
Args:
min_speed (int): The minim speed the motor should be moving at
"""
if abs(self.speed()) <= abs(min_speed):
return True
return False
def __repr__(self):
return "Motor Properties:\nPort: " + str(
self.port) + "\nDefault Direction: " + str(self.direction)
class Spik3r(EV3Brick):
def __init__(self,
sting_motor_port: Port = Port.D,
go_motor_port: Port = Port.B,
claw_motor_port: Port = Port.A,
touch_sensor_port: Port = Port.S1,
color_sensor_port: Port = Port.S3,
ir_sensor_port: Port = Port.S4,
ir_beacon_channel: int = 1):
self.sting_motor = Motor(port=sting_motor_port,
positive_direction=Direction.CLOCKWISE)
self.go_motor = Motor(port=go_motor_port,
positive_direction=Direction.CLOCKWISE)
self.claw_motor = Motor(port=claw_motor_port,
positive_direction=Direction.CLOCKWISE)
self.ir_sensor = InfraredSensor(port=ir_sensor_port)
self.ir_beacon_channel = ir_beacon_channel
self.touch_sensor = TouchSensor(port=touch_sensor_port)
self.color_sensor = ColorSensor(port=color_sensor_port)
def sting_by_ir_beacon(self):
while True:
ir_buttons_pressed = set(
self.ir_sensor.buttons(channel=self.ir_beacon_channel))
if ir_buttons_pressed == {Button.BEACON}:
self.sting_motor.run_angle(speed=-750,
rotation_angle=220,
then=Stop.HOLD,
wait=False)
self.speaker.play_file(file=SoundFile.EV3)
self.sting_motor.run_time(speed=-1000,
time=1000,
then=Stop.HOLD,
wait=True)
self.sting_motor.run_time(speed=1000,
time=1000,
then=Stop.HOLD,
wait=True)
while Button.BEACON in self.ir_sensor.buttons(
channel=self.ir_beacon_channel):
pass
def be_noisy_to_people(self):
while True:
if self.color_sensor.reflection() > 30:
for i in range(4):
self.speaker.play_file(file=SoundFile.ERROR_ALARM)
def pinch_if_touched(self):
while True:
if self.touch_sensor.pressed():
self.claw_motor.run_time(speed=500,
time=1000,
then=Stop.HOLD,
wait=True)
self.claw_motor.run_time(speed=-500,
time=0.3 * 1000,
then=Stop.HOLD,
wait=True)
def keep_driving_by_ir_beacon(self):
while True:
ir_buttons_pressed = set(
self.ir_sensor.buttons(channel=self.ir_beacon_channel))
if ir_buttons_pressed == {Button.RIGHT_UP, Button.LEFT_UP}:
self.go_motor.run(speed=910)
elif ir_buttons_pressed == {Button.RIGHT_UP}:
self.go_motor.run(speed=-1000)
else:
self.go_motor.stop()
def main(self):
self.screen.load_image(ImageFile.EVIL)
# FIXME: OSError: [Errno 5] EIO:
# Unexpected hardware input/output error with a motor or sensor:
# --> Try unplugging the sensor or motor and plug it back in again.
# --> To see which sensor or motor is causing the problem,
# check the line in your script that matches
# the line number given in the 'Traceback' above.
# --> Try rebooting the hub/brick if the problem persists.
Process(target=self.be_noisy_to_people).start()
Process(target=self.sting_by_ir_beacon).start()
Process(target=self.pinch_if_touched).start()
self.keep_driving_by_ir_beacon()
from pybricks.parameters import Port, Stop, Direction, Button, Color, SoundFile, ImageFile, Align
from pybricks.tools import wait, StopWatch, DataLog
from pybricks.robotics import DriveBase
import time
# 모터 선언
motorB = Motor(Port.B)
motorC = Motor(Port.C)
# 컬러 센서 선언
color_sensor = ColorSensor(Port.S1)
# N번째 검은 선에서 멈춘다.
N = 3
# 반복문
for count in range(N):
motorB.run(300)
motorC.run(300)
# 반사값이 15보다 작으면 while 종료
while True:
if color_sensor.reflection() < 15:
break
brick.sound.beep()
# 모터 정지
motorB.stop(Stop.BRAKE)
motorC.stop(Stop.BRAKE)
#!/usr/bin/env pybricks-micropython
from pybricks import ev3brick as brick
from pybricks.ev3devices import (Motor, TouchSensor, ColorSensor,
InfraredSensor, UltrasonicSensor, GyroSensor)
from pybricks.parameters import (Port, Stop, Direction, Button, Color,
SoundFile, ImageFile, Align)
from pybricks.tools import print, wait, StopWatch
from pybricks.robotics import DriveBase
left_motor = Motor(Port.B)
right_motor = Motor(Port.C)
touch_sensor = TouchSensor(Port.S1)
while (not touch_sensor.pressed()):
left_motor.run(360)
right_motor.run(360)
wait(10)
while (touch_sensor.pressed()):
wait(10)
left_motor.stop(Stop.BRAKE)
right_motor.stop(Stop.BRAKE)
wait(1000)
from pybricks.tools import wait, StopWatch, DataLog
from pybricks.robotics import DriveBase
from pybricks.media.ev3dev import SoundFile, ImageFile
esquerda = Motor(Port.B, Direction.COUNTERCLOCKWISE)
direita = Motor(Port.C, Direction.COUNTERCLOCKWISE)
MotorMA = Motor(Port.A)
MotorMD = Motor(Port.D)
# EsqCor = ColorSensor(Port.S1)
# DirCor = ColorSensor(Port.S4)
gabriel = DriveBase(esquerda, direita, wheel_diameter=100,
axle_track=166.2) #Ajustar valores
ev3 = EV3Brick()
while True:
if Button.LEFT in ev3.buttons.pressed():
MotorMA.run(-400)
if Button.RIGHT in ev3.buttons.pressed():
MotorMA.run(400)
if Button.DOWN in ev3.buttons.pressed():
MotorMD.run(-400)
if Button.UP in ev3.buttons.pressed():
MotorMD.run(400)
if ev3.buttons.pressed() == []:
MotorMD.stop()
MotorMA.stop()
if Button.CENTER in ev3.buttons.pressed():
MotorMD.reset_angle(0)
MotorMA.reset_angle(0)
class Dinor3x(EV3Brick):
"""
Challenges:
- Can you make DINOR3X remote controlled with the IR-Beacon?
- Can you attach a colorsensor to DINOR3X, and make it behave differently
depending on which color is in front of the sensor
(red = walk fast, white = walk slow, etc.)?
"""
def __init__(
self,
left_motor_port: Port = Port.B, right_motor_port: Port = Port.C,
jaw_motor_port: Port = Port.A,
touch_sensor_port: Port = Port.S1,
ir_sensor_port: Port = Port.S4, ir_beacon_channel: int = 1):
self.left_motor = Motor(port=left_motor_port,
positive_direction=Direction.CLOCKWISE)
self.right_motor = Motor(port=right_motor_port,
positive_direction=Direction.CLOCKWISE)
self.jaw_motor = Motor(port=jaw_motor_port,
positive_direction=Direction.CLOCKWISE)
self.touch_sensor = TouchSensor(port=touch_sensor_port)
self.ir_sensor = InfraredSensor(port=ir_sensor_port)
self.ir_beacon_channel = ir_beacon_channel
def calibrate_legs(self):
self.left_motor.run(speed=100)
self.right_motor.run(speed=200)
while self.touch_sensor.pressed():
pass
self.left_motor.hold()
self.right_motor.hold()
self.left_motor.run(speed=400)
while not self.touch_sensor.pressed():
pass
self.left_motor.hold()
self.left_motor.run_angle(
rotation_angle=-0.2 * 360,
speed=500,
then=Stop.HOLD,
wait=True)
self.right_motor.run(speed=400)
while not self.touch_sensor.pressed():
pass
self.right_motor.hold()
self.right_motor.run_angle(
rotation_angle=-0.2 * 360,
speed=500,
then=Stop.HOLD,
wait=True)
self.left_motor.reset_angle(angle=0)
self.right_motor.reset_angle(angle=0)
def roar(self):
self.speaker.play_file(file=SoundFile.T_REX_ROAR)
self.jaw_motor.run_angle(
speed=400,
rotation_angle=-60,
then=Stop.HOLD,
wait=True)
# FIXME: jaw doesn't close
for i in range(12):
self.jaw_motor.run_time(
speed=-400,
time=0.05 * 1000,
then=Stop.HOLD,
wait=True)
self.jaw_motor.run_time(
speed=400,
time=0.05 * 1000,
then=Stop.HOLD,
wait=True)
self.jaw_motor.run(speed=200)
sleep(0.5)
def close_mouth(self):
self.jaw_motor.run(speed=200)
sleep(1)
self.jaw_motor.stop()
def walk_until_blocked(self):
self.left_motor.run(speed=-400)
self.right_motor.run(speed=-400)
while self.ir_sensor.distance() >= 25:
pass
self.left_motor.stop()
self.right_motor.stop()
def run_away(self):
self.left_motor.run_angle(
speed=750,
rotation_angle=3 * 360,
then=Stop.BRAKE,
wait=False)
self.right_motor.run_angle(
speed=750,
rotation_angle=3 * 360,
then=Stop.BRAKE,
wait=True)
def jump(self):
"""
Dinor3x Mission 02 Challenge: make it jump
"""
...
# TRANSLATED FROM EV3-G MY BLOCKS
# -------------------------------
def leg_adjust(
self,
cyclic_degrees: float,
speed: float = 1000,
leg_offset_percent: float = 0,
mirrored_adjust: bool = False,
brake: bool = True):
...
def leg_to_pos(
self,
speed: float = 1000,
left_position: float = 0,
right_position: float = 0):
self.left_motor.brake()
self.right_motor.brake()
self.left_motor.run_angle(
speed=speed,
rotation_angle=left_position -
cyclic_position_offset(
rotation_sensor=self.left_motor.angle(),
cyclic_degrees=360),
then=Stop.BRAKE,
wait=True)
self.right_motor.run_angle(
speed=speed,
rotation_angle=right_position -
cyclic_position_offset(
rotation_sensor=self.right_motor.angle(),
cyclic_degrees=360),
then=Stop.BRAKE,
wait=True)
def turn(self, speed: float = 1000, n_steps: int = 1):
...
def walk(self, speed: float = 1000):
...
def walk_steps(self, speed: float = 1000, n_steps: int = 1):
...
class Puppy:
# Constants for leg angle
HALF_UP_ANGLE = 25
STAND_UP_ANGLE = 65
STRETCH_ANGLE = 125
# Loads the different eyes
HEART_EYES = Image(ImageFile.LOVE)
SQUINTY_EYES = Image(ImageFile.TEAR)
def __init__(self):
# Sets up the brick
self.ev3 = EV3Brick()
# Identifies which motor is connected to which ports
self.left_leg_motor = Motor(Port.D, Direction.COUNTERCLOCKWISE)
self.right_leg_motor = Motor(Port.A, Direction.COUNTERCLOCKWISE)
# Sets up the gear ratio (automatically translates it to the correct angle)
self.head_motor = Motor(Port.C,
Direction.COUNTERCLOCKWISE,
gears=[[1, 24], [12, 36]])
# Sets up touch sensor
self.touch_sensor = TouchSensor(Port.S1)
# Sets up constants for the eye
self.eyes_timer_1 = StopWatch()
self.eyes_timer_1_end = 0
self.eyes_timer_2 = StopWatch()
self.eyes_timer_2_end = 0
self.eyes_closed = False
def movements(self):
self.ev3.screen.load_image(ImageFile.EV3_ICON)
self.ev3.light.on(Color.ORANGE)
dog_pat = 0
# Movement interactions
while True:
buttons = self.ev3.buttons.pressed()
if Button.CENTER in buttons:
break
elif Button.UP in buttons:
self.head_motor.run(20)
elif Button.DOWN in buttons:
self.head_motor.run(-20)
elif self.touch_sensor.pressed():
self.ev3.speaker.play_file(SoundFile.DOG_BARK_1)
self.eyes = self.HEART_EYES
self.sit_down()
dog_pat += 1
print(dog_pat)
elif dog_pat == 3:
self.go_to_bathroom()
dog_pat -= 3
print(dog_pat)
else:
self.head_motor.stop()
wait(100)
self.head_motor.stop()
self.head_motor.reset_angle(0)
self.ev3.light.on(Color.GREEN)
# Below this line I honestly have no clue how it works. It came from the boiler plate of functions
def move_head(self, target):
self.head_motor.run_target(20, target)
@property
def eyes(self):
return self._eyes
@eyes.setter
def eyes(self, value):
if value != self._eyes:
self._eyes = value
self.ev3.screen.load_image(value)
def update_eyes(self):
if self.eyes_timer_1.time() > self.eyes_timer_1_end:
self.eyes_timer_1.reset()
if self.eyes == self.SLEEPING_EYES:
self.eyes_timer_1_end = urandom.randint(1, 5) * 1000
self.eyes = self.TIRED_RIGHT_EYES
else:
self.eyes_timer_1_end = 250
self.eyes = self.SLEEPING_EYES
if self.eyes_timer_2.time() > self.eyes_timer_2_end:
self.eyes_timer_2.reset()
if self.eyes != self.SLEEPING_EYES:
self.eyes_timer_2_end = urandom.randint(1, 10) * 1000
if self.eyes != self.TIRED_LEFT_EYES:
self.eyes = self.TIRED_LEFT_EYES
else:
self.eyes = self.TIRED_RIGHT_EYES
def stand_up(self):
self.left_leg_motor.run_target(100, self.HALF_UP_ANGLE, wait=False)
self.right_leg_motor.run_target(100, self.HALF_UP_ANGLE)
while not self.left_leg_motor.control.done():
wait(100)
self.left_leg_motor.run_target(50, self.STAND_UP_ANGLE, wait=False)
self.right_leg_motor.run_target(50, self.STAND_UP_ANGLE)
while not self.left_leg_motor.control.done():
wait(100)
wait(500)
def sit_down(self):
self.left_leg_motor.run(-50)
self.right_leg_motor.run(-50)
wait(1000)
self.left_leg_motor.stop()
self.right_leg_motor.stop()
wait(100)
def go_to_bathroom(self):
self.eyes = self.SQUINTY_EYES
self.stand_up()
wait(100)
self.right_leg_motor.run_target(100, self.STRETCH_ANGLE)
wait(800)
self.ev3.speaker.play_file(SoundFile.HORN_1)
wait(1000)
for _ in range(3):
self.right_leg_motor.run_angle(100, 20)
self.right_leg_motor.run_angle(100, -20)
self.right_leg_motor.run_target(100, self.STAND_UP_ANGLE)
def run(self):
self.movements()
self.eyes = self.SLEEPING_EYES
brick.light(Color.GREEN) # Brick LED becomes green
# Run the motors up to 'motorSpeed' degrees per second:
motorL.run(motorSpeed)
motorR.run(motorSpeed)
while started is True:
# Check if there is enough space; if not, stop, turn 90 degrees, then start running again
if ultrasonic.distance() < minDistance:
# Stop the motors:
tempSpeed = motorSpeed
for i in range(1, 100):
tempSpeed = tempSpeed - 1
motorL.run(tempSpeed)
motorR.run(tempSpeed)
#wait(100)
motorL.stop()
motorR.stop()
# Turn 90 degrees until there is enough space to start running again:
death = 4
while ultrasonic.distance() < minDistance:
#motorL.set_run_settings(maxSpeed, 400)
#motorR.set_run_settings(maxSpeed, 400)
motorL.run_angle(steeringSpeed, -174, Stop.COAST, False)
motorR.run_angle(steeringSpeed, +174)
death -= 1
if (death == 0):
started = False
break
# Run the motors up to 'motorSpeed' degrees per second:
if death != 0:
#!/usr/bin/env pybricks-micropython
from pybricks import ev3brick as brick
from pybricks.ev3devices import (Motor, TouchSensor, ColorSensor,
InfraredSensor, UltrasonicSensor, GyroSensor)
from pybricks.parameters import (Port, Stop, Direction, Button, Color,
SoundFile, ImageFile, Align)
from pybricks.tools import print, wait, StopWatch
from pybricks.robotics import DriveBase
# Write your program here
brick.sound.beep()
brick.light(Color.RED)
motorR = Motor(Port.B)
motorL = Motor(Port.C)
ultraSense = UltrasonicSensor(Port.S3)
dist = 69
sped = 69
motorL.run(sped)
motorR.run(sped)
while dist < ultraSense.distance():
brick.sound.beep(69)
motorR.stop(Stop.BRAKE)
motorL.stop(Stop.BRAKE)
class R3ptar:
"""
R3ptar can be driven around by the IR Remote Control,
strikes when the Beacon button is pressed,
and hisses when the Touch Sensor is pressed
(inspiration from LEGO Mindstorms EV3 Home Edition: R3ptar: Tutorial #4)
"""
def __init__(self,
steering_motor_port: Port = Port.A,
driving_motor_port: Port = Port.B,
striking_motor_port: Port = Port.D,
touch_sensor_port: Port = Port.S1,
ir_sensor_port: Port = Port.S4,
ir_beacon_channel: int = 1):
self.ev3_brick = EV3Brick()
self.steering_motor = Motor(port=steering_motor_port,
positive_direction=Direction.CLOCKWISE)
self.driving_motor = Motor(port=driving_motor_port,
positive_direction=Direction.CLOCKWISE)
self.striking_motor = Motor(port=striking_motor_port,
positive_direction=Direction.CLOCKWISE)
self.touch_sensor = TouchSensor(port=touch_sensor_port)
self.ir_sensor = InfraredSensor(port=ir_sensor_port)
self.ir_beacon_channel = ir_beacon_channel
def drive_by_ir_beacon(
self,
speed: float = 1000, # mm/s
):
ir_beacons_pressed = \
set(self.ir_sensor.buttons(channel=self.ir_beacon_channel))
if ir_beacons_pressed == {Button.LEFT_UP, Button.RIGHT_UP}:
self.driving_motor.run(speed=speed)
elif ir_beacons_pressed == {Button.LEFT_DOWN, Button.RIGHT_DOWN}:
self.driving_motor.run(speed=-speed)
elif ir_beacons_pressed == {Button.LEFT_UP}:
self.steering_motor.run(speed=-500)
self.driving_motor.run(speed=speed)
elif ir_beacons_pressed == {Button.RIGHT_UP}:
self.steering_motor.run(speed=500)
self.driving_motor.run(speed=speed)
elif ir_beacons_pressed == {Button.LEFT_DOWN}:
self.steering_motor.run(speed=-500)
self.driving_motor.run(speed=-speed)
elif ir_beacons_pressed == {Button.RIGHT_DOWN}:
self.steering_motor.run(speed=500)
self.driving_motor.run(speed=-speed)
else:
self.steering_motor.hold()
self.driving_motor.stop()
def strike_by_ir_beacon(self, speed: float = 1000):
if Button.BEACON in \
self.ir_sensor.buttons(channel=self.ir_beacon_channel):
self.striking_motor.run_time(speed=speed,
time=1000,
then=Stop.COAST,
wait=True)
self.striking_motor.run_time(speed=-speed,
time=1000,
then=Stop.COAST,
wait=True)
while Button.BEACON in \
self.ir_sensor.buttons(channel=self.ir_beacon_channel):
pass
def hiss_if_touched(self):
if self.touch_sensor.pressed():
self.ev3_brick.speaker.play_file(file=SoundFile.SNAKE_HISS)
robot.drive(800, 0)
move = 1
# Keep driving until the random time has passed or an object is
# detected. If an object is detected the "checking" variable
# will be set to "False."
while checking and timer.time() < random_time:
checking = ultrasonic_sensor.distance() > 400
wait(10)
# Stop driving.
robot.drive(0, 0)
# Check if the object is closer than 250 mm.
if ultrasonic_sensor.distance() < 250:
# Roar and move the head forward to bite.
head_motor.dc(-100)
ev3.speaker.play_file(SoundFile.T_REX_ROAR)
wait(250)
head_motor.stop()
wait(1000)
else:
# Move the head and hiss.
head_motor.dc(-100)
wait(100)
head_motor.stop()
ev3.speaker.play_file(SoundFile.SNAKE_HISS)
# Reset the head motor to its initial position.
head_motor.run_target(1200, 0)
ev3.speaker.beep()
US = UltrasonicSensor(Port.S1)
LeftMotor = Motor(Port.A, positive_direction=Direction.CLOCKWISE, gears=None)
RightMotor = Motor(Port.B, positive_direction=Direction.CLOCKWISE, gears=None)
SpinMotor = Motor(Port.C, positive_direction=Direction.CLOCKWISE, gears=None)
drive = DriveBase(LeftMotor, RightMotor, 55, 203)
var = 1
escape_dist = 800
escape_ang = 360
while var > 0:
if US.distance() < 500:
drive.stop()
while US.distance() < escape_dist:
drive.drive(100, 180)
wait(10)
# ev3.screen.print(US.distance())
# print(US.distance(),',',drive.angle())
SpinMotor.run(1080)
ev3.speaker.beep()
# if drive.angle() > escape_ang:
# escape_dist = escape_dist - 200
# escape_ang = escape_ang + 360
# ev3.speaker.say('adjusting parameters')
SpinMotor.stop()
# escape_dist = 800
# reset_angle
# ev3.speaker.play_notes('C5/4_','C6/4')
drive.drive(500, 0)
print(US.distance())
class Bri6Pack:
def __init__(self):
# Initialize the EV3 Brick.
self.ev3 = EV3Brick()
self.ev3.speaker.set_speech_options(voice='f3')
# large motors
self.left_motor = Motor(Port.A,
positive_direction=Direction.COUNTERCLOCKWISE,
gears=None)
self.right_motor = Motor(Port.B,
positive_direction=Direction.COUNTERCLOCKWISE,
gears=None)
# small (medium) motors
self.small_motor_left = Motor(Port.C,
positive_direction=Direction.CLOCKWISE,
gears=None)
self.small_motor_right = Motor(Port.D,
positive_direction=Direction.CLOCKWISE,
gears=None)
# gyro sensor
self.gyro_sensor = GyroSensor(Port.S3, Direction.CLOCKWISE)
# Initialize the drive base.
self.drive_base = DriveBase(self.left_motor,
self.right_motor,
wheel_diameter=93.5,
axle_track=120)
return
"""
A proportional–integral–derivative controller
"""
def drive_pid(self, speed, distance):
isDebug = False
# the sign of the input speed is ignored
speed = abs(speed)
# direction is indicated by the sign of the input distance
# postive distance ==> move forward
# negative distance ==> move backward
if (distance < 0):
speed = -speed
distance = -distance
drive_base = self.drive_base
gyro_sensor = self.gyro_sensor
# resets the estimated driven distance and angle to 0
drive_base.reset()
# reset gyro sensor
# a = gyro_sensor.speed()
# b = gyro_sensor.angle()
gyro_sensor.reset_angle(0)
err_p = 0.0
err_i = 0.0
err_d = 0.0
err_prev = 0.0
i = 0
# do not use factor
useSpeedFactor = False
factor = 1.0
if useSpeedFactor:
factor = abs(speed / 300.0)
deltaT = 0.02
total_distance = abs(distance)
gyro_PID_distance = total_distance * 0.8
# use gyro_PID for the first half
while abs(drive_base.distance()) <= gyro_PID_distance:
err_p = gyro_sensor.angle()
err_i += err_p * deltaT
err_d = (err_p - err_prev) / deltaT
kp = 1 * factor
ki = 4 * factor
kd = 0.01 * factor
# limit the speed err_i can grow (exponential decay)
if abs(err_i) >= 100:
err_i *= math.exp(-abs(err_i / 100))
turn_rate = -(kp * err_p + ki * err_i + kd * err_d)
drive_base.drive(speed, turn_rate)
time.sleep(0.05)
# debugging output
if (i % 1 == 0 and isDebug):
print(
"Count={}, Distance={}, P={}, I={}, D={}, CP={}, CI={}, CD={}, turn rate={}"
.format(i, drive_base.distance(), err_p, err_i, err_d,
kp * err_p, ki * err_i, kd * err_d, turn_rate))
i += 1
err_prev = err_p
# finish the distance with motor_PID
motor_PID_distance = total_distance - abs(drive_base.distance())
drive_base.stop()
drive_base.settings(straight_speed=speed)
if speed < 0:
motor_PID_distance *= -1
drive_base.straight(motor_PID_distance)
if isDebug:
print("Final: Distance={} gyroAngle={}, speed={}, mDist={}".format(
drive_base.distance(), gyro_sensor.angle(), speed,
motor_PID_distance))
## stall_tolerances(speed, time)
## If the controller cannot reach this "speed"
## for some "time" even with maximum "actuation", it is stalled.
# robot.distance_control.stall_tolerances(speed, 100)
# robot.distance_control.limits(actuation = 40)
def check_stall(self):
if self.drive_base.distance_control.stalled():
print("STALLED")
self.drive_base.stop()
return True
def reset_motors(self):
self.drive_base.stop()
self.small_motor_left.stop()
self.small_motor_right.stop()
def count_down(self, sec):
for i in range(sec, 0, -1):
#print(i)
if i == 5:
# this is not accurate as speaking takes time
self.ev3.speaker.say("5 seconds left")
else:
time.sleep(1)
def say(self, words):
self.ev3.speaker.say(words)
"""
return inches*25.4
# Initialize our drive motors and a drive base
# 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 direction passed to
# the motor initialization to achieve this.
arm_motor = Motor(ARM_MOTOR_PORT, Direction.COUNTERCLOCKWISE)
left_motor = Motor(LEFT_MOTOR_PORT)
right_motor = Motor(RIGHT_MOTOR_PORT)
# 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.stop()
arm_motor.run_until_stalled(-180)
arm_motor.stop()
# Initialize our drive base and drive 24.0 inches at 5 inches/sec.
robot = MyDriveBase(left_motor=left_motor, right_motor=right_motor,
wheel_diameter=WHEEL_DIAMETER_MM, axle_track=AXLE_TRACK_MM)
robot.drive_distance(inches=24.0, speed=5.0, steering=0)
# Raise the arm until we stall, which will hopefully raise the lever
# at the base of the crane.
arm_motor.stop()
arm_motor.run_until_stalled(90)
arm_motor.stop()
# Sleep for a little bit while the arm is raised so that the block
reset_all()
while True:
PID(-700,0,1,1,1)
stop1()
robot.stop()
elif Button.DOWN in brick.buttons():
while not Button.CENTER in brick.buttons():
first_gyro.reset_angle(0)
second_gyro.reset_angle(0)
small_left_m = Motor(Port.A)
i=0
while i!= 1500:
small_left_m.dc(40)
i+=1
stop1()
small_left_m.stop()
time.sleep(0.5)
left_M.reset_angle(0)
right_M.reset_angle(0)
while motor_avarge()<=740:
PID(150,0,8,1,1)
stop1()
robot.stop()
time.sleep(0.1)
left_M.reset_angle(0)
right_M.reset_angle(0)
while motor_avarge()<=720:
followline(100,0.5,1,1)
stop1()
robot.stop()
time.sleep(0.5)
#!/usr/bin/env pybricks-micropython
from pybricks.hubs import EV3Brick
from pybricks.ev3devices import (Motor, TouchSensor, ColorSensor,
InfraredSensor, UltrasonicSensor, GyroSensor)
from pybricks.parameters import (Port, Stop, Direction, Button, Color,
SoundFile, ImageFile, Align)
from pybricks.tools import print, wait, StopWatch
from pybricks.robotics import DriveBase
ev3 = EV3Brick()
ultrasonicSensor = UltrasonicSensor(Port.S2)
left_motor = Motor(Port.A)
right_motor = Motor(Port.D)
program_running = True
distance = ultrasonicSensor.distance()
left_motor.run(800)
right_motor.run(800)
while distance > 195:
distance = ultrasonicSensor.distance()
print(distance)
left_motor.stop()
right_motor.stop()
def stop2():
if Button.CENTER in brick.buttons():
left_M.stop()
right_M.stop()
'''small_left_m.stop()'''
'''small_right_m.stop() '''
while True:
if first_cls.color() == Color.RED:
while not Button.CENTER in brick.buttons():
reset_all()
while motor_avarge()<=1200:
PID(150,0,5,1,1)
stop1()
robot.stop()
reset_all()
time.sleep(1)
while motor_avarge()>=-270:
PID(-400,0,1,1,1)
stop1()
robot.stop()
reset_all()
while True:
PID(-500,-210,1,1,1)
stop1()
elif Button.LEFT in brick.buttons():
while not Button.CENTER in brick.buttons():
reset_all()
while motor_avarge()<=970:
PID(200,0,8,1,1)
stop1()
robot.stop()
reset_all()
while motor_avarge()>=-450:
PID(-200,0,1,1,1)
stop1()
robot.stop()
reset_all()
while True:
PID(-500,-200,1,1,1)
stop1()
elif Button.UP in brick.buttons():
while not Button.CENTER in brick.buttons():
reset_all()
while motor_avarge()<=780:
PID(150,0,8,1,1)
stop1()
robot.stop()
left_M.reset_angle(0)
right_M.reset_angle(0)
while motor_avarge()<=470:
followline(100,1.2,1,1)
stop1()
robot.stop()
time.sleep(0.1)
first_gyro.reset_angle(0)
second_gyro.reset_angle(0)
while first_gyro.angle()<=25:
robot.drive(10,-100)
stop1()
robot.stop()
reset_all()
while motor_avarge()<=1000:
PID(160,0,8,1,1)
stop1()
robot.stop()
time.sleep(0.2)
left_M.reset_angle(0)
right_M.reset_angle(0)
if second_cls.reflection()>=65:
while motor_avarge()<=500:
followline(100,0.6,1,1)
stop1()
robot.stop()
elif second_cls.reflection()<=10:
while motor_avarge()<=460:
followline(100,0.6,1,1)
stop1()
robot.stop()
else:
while motor_avarge()<=420:
PID(100,-4,1,1,1)
stop1()
robot.stop()
time.sleep(0.3)
left_M.reset_angle(0)
right_M.reset_angle(0)
while motor_avarge()>=-1500:
PID(-300,0,1,1,1)
stop1()
robot.stop()
while first_gyro.angle()>=-17:
robot.drive(10,100)
stop1()
robot.stop()
reset_all()
while True:
PID(-700,0,1,1,1)
stop1()
robot.stop()
elif Button.DOWN in brick.buttons():
while not Button.CENTER in brick.buttons():
first_gyro.reset_angle(0)
second_gyro.reset_angle(0)
small_left_m = Motor(Port.A)
i=0
while i!= 1500:
small_left_m.dc(40)
i+=1
stop1()
small_left_m.stop()
time.sleep(0.5)
left_M.reset_angle(0)
right_M.reset_angle(0)
while motor_avarge()<=740:
PID(150,0,8,1,1)
stop1()
robot.stop()
time.sleep(0.1)
left_M.reset_angle(0)
right_M.reset_angle(0)
while motor_avarge()<=720:
followline(100,0.5,1,1)
stop1()
robot.stop()
time.sleep(0.5)
i=0
while i!= 2000:
small_left_m.dc(-40)
i+=1
stop1()
small_left_m.stop()
reset_all()
while motor_avarge()<=1000:
PID(150,0,1,1,1)
stop1()
robot.stop()
time.sleep(1)
reset_all()
while True:
PID(-500,0,1,1,1)
stop1()
elif Button.RIGHT in brick.buttons():
while not Button.CENTER in brick.buttons():
reset_all()
while motor_avarge()<=740:
PID(150,0,1,1,1)
stop1()
robot.stop()
time.sleep(0.1)
left_M.reset_angle(0)
right_M.reset_angle(0)
while motor_avarge()<=870:
followline(150,0.6,1,1)
stop1()
robot.stop()
reset_all()
while motor_avarge()<=400:
PID(150,0,1,1,1)
stop1()
robot.stop()
reset_all()
while first_gyro.angle()<=100:
robot.drive(10,-100)
stop1()
robot.stop()
i=0
while i<=110:
robot.drive(-100,0)
i+=1
stop1()
robot.stop()
reset_all()
while motor_avarge()<=270:
PID(150,0,1,1,1)
stop1()
robot.stop()
reset_all()
while first_gyro.angle()<=27:
robot.drive(10,-100)
stop1()
robot.stop()
reset_all()
while motor_avarge()<=600:
followline(100,1,1,1)
stop1()
robot.stop()
reset_all()
while first_gyro.angle()<=170:
robot.drive(10,-100)
stop1()
robot.stop()
while motor_avarge()>=-1230:
robot.drive(-1000,0)
stop1()
robot.stop()
reset_all()
while True:
if motor_avarge()>0:
while motor_avarge()>0:
robot.drive(-10,0)
stop1()
robot.stop()
elif motor_avarge()<0:
while motor_avarge()<0:
robot.drive(-10,0)
stop1()
robot.stop()
while motor_avarge()>=-1300:
robot.drive(-2000,0)
stop1()
robot.stop()
reset_all()
class Kraz33Hors3:
def __init__(self,
back_foot_motor_port: Port = Port.C,
front_foot_motor_port: Port = Port.B,
gear_motor_port: Port = Port.A,
touch_sensor_port: Port = Port.S1,
color_sensor_port: Port = Port.S3,
ir_sensor_port: Port = Port.S4,
ir_beacon_channel: int = 1):
self.front_foot_motor = Motor(port=front_foot_motor_port,
positive_direction=Direction.CLOCKWISE)
self.back_foot_motor = Motor(
port=back_foot_motor_port,
positive_direction=Direction.COUNTERCLOCKWISE)
self.gear_motor = Motor(port=gear_motor_port)
self.touch_sensor = TouchSensor(port=touch_sensor_port)
self.color_sensor = ColorSensor(port=color_sensor_port)
self.ir_sensor = InfraredSensor(port=ir_sensor_port)
self.ir_beacon_channel = ir_beacon_channel
def drive_once_by_ir_beacon(
self,
speed: float = 1000 # deg/s
):
ir_beacons_pressed = set(
self.ir_sensor.buttons(channel=self.ir_beacon_channel))
# forward
if ir_beacons_pressed == {Button.LEFT_UP, Button.RIGHT_UP}:
self.front_foot_motor.run_time(
speed=speed,
time=1000, # ms
then=Stop.COAST,
wait=False)
self.back_foot_motor.run_time(
speed=speed,
time=1000, # ms
then=Stop.COAST,
wait=True)
# backward
elif ir_beacons_pressed == {Button.LEFT_DOWN, Button.RIGHT_DOWN}:
self.front_foot_motor.run_time(
speed=-speed,
time=1000, # ms
then=Stop.COAST,
wait=False)
self.back_foot_motor.run_time(
speed=-speed,
time=1000, # ms
then=Stop.COAST,
wait=True)
# move crazily
elif ir_beacons_pressed == {Button.BEACON}:
self.gear_motor.run(speed=speed)
self.front_foot_motor.run_time(
speed=speed / 3,
time=1000, # ms
then=Stop.COAST,
wait=False)
self.back_foot_motor.run_time(
speed=-speed / 3,
time=1000, # ms
then=Stop.COAST,
wait=True)
else:
self.gear_motor.stop()
def keep_driving_by_ir_beacon(
self,
speed: float = 1000 # deg/s
):
while True:
self.drive_once_by_ir_beacon(speed=speed)
def back_whenever_touched(
self,
speed: float = 1000 # deg/s
):
while True:
if self.touch_sensor.pressed():
self.front_foot_motor.run_time(
speed=-speed,
time=1000, # ms
then=Stop.COAST,
wait=False)
self.back_foot_motor.run_time(
speed=-speed,
time=1000, # ms
then=Stop.COAST,
wait=True)
def main(
self,
speed: float = 1000 # deg/s
):
Thread(target=self.back_whenever_touched).start()
self.keep_driving_by_ir_beacon(speed=speed)
waitBC() # make sure motors are not moving
while oc < 4:
oc += 1
cycles = 0
while (cycles < 2):
cycles += 1
print("Cycles: %d,%d" % (oc,cycles))
t1 = watch.time() / 1000
robot.drive_time(100, 45, 8000)
t2 = watch.time() / 1000
driveTime = t2-t1
print("CW drive time: %5.3f" % (driveTime))
DTFlag = True
sleep(1) # just in case not quite done
mB.stop(Stop.HOLD)
mC.stop(Stop.HOLD)
waitBC()
while (cycles < 4):
cycles += 1
print("Cycles: %d,%d" % (oc,cycles))
t1 = watch.time() / 1000
robot.drive_time(100, -45, 8000)
t2 = watch.time() / 1000
driveTime = t2-t1
print("CCW drive time: %5.3f" % (driveTime))
DTFlag = True
sleep(1) # just in case not quite done
mB.stop(Stop.HOLD)
mC.stop(Stop.HOLD)
robot.settings(250, 300, 100, 600) StopAtWhiteLine() robot.straight(175) #robot turns toward the wall and reveres: robot.turn(-120) robot.straight(-180) #goes off the wall and turns to the wheel robot.straight(200) robot.turn(-65) robot.stop() #Goes foreward to flip the wheel medium_left.run_time(-200, 5000) robot.straight(80) medium_left.run_time(150, 5000) medium_left.stop() #Large Tire Flip #StopAtWhiteLineRightMotorBackwards() robot.turn(30) robot.straight(-50) medium_left.run_time(-200, 4000) robot.straight(110) medium_left.run_time(150, 6500) #back after both tire flip robot.turn(20) robot.straight(-550) robot.straight(150) robot.turn(-125) robot.straight(300) robot.stop()
def main():
ev3 = EV3Brick()
ev3.speaker.beep()
ev3.screen.clear()
ev3.light.on(Color.YELLOW)
colors = ColorSensor(Port.S3)
left_motor = Motor(Port.B, Direction.CLOCKWISE)
right_motor = Motor(Port.C, Direction.CLOCKWISE)
startup = True
direction = 1
ev3.light.off()
while (not ev3.buttons.pressed()):
if (not checkForLine(ev3, colors)):
left_motor.stop()
right_motor.stop()
# no more line! either:
# a) the line turned
# b) we were not running straight and went to side of line
# c) there was never a line to begin with
# (b)
newDir = findLineSmallMotion(ev3, colors, left_motor, right_motor,
direction)
if (0 != newDir):
direction = newDir
else:
# (a)
newDir = findLineLargeMotion(ev3, colors, left_motor,
right_motor, direction)
if (0 != newDir):
direction = -1 * newDir
else:
startup = True
# (c)
if (startup):
ev3.speaker.beep()
ev3.speaker.say("put me on the line")
while (not checkForLine(ev3, colors)):
wait(100)
ev3.speaker.say("press any button to start")
while (not ev3.buttons.pressed()):
wait(10)
continue
startup = False
# go
left_motor.run(400)
right_motor.run(400)
wait(10)
class Gripp3r(IRBeaconRemoteControlledTank, EV3Brick):
WHEEL_DIAMETER = 26
AXLE_TRACK = 115
def __init__(self,
left_motor_port: Port = Port.B,
right_motor_port: Port = Port.C,
grip_motor_port: Port = Port.A,
touch_sensor_port: Port = Port.S1,
ir_sensor_port: Port = Port.S4,
ir_beacon_channel: int = 1):
super().__init__(wheel_diameter=self.WHEEL_DIAMETER,
axle_track=self.AXLE_TRACK,
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.drive_base.settings(
straight_speed=750, # milimeters per second
straight_acceleration=750,
turn_rate=90, # degrees per second
turn_acceleration=90)
self.grip_motor = Motor(port=grip_motor_port,
positive_direction=Direction.CLOCKWISE)
self.touch_sensor = TouchSensor(port=touch_sensor_port)
self.ir_sensor = InfraredSensor(port=ir_sensor_port)
self.ir_beacon_channel = ir_beacon_channel
def grip_or_release_by_ir_beacon(self, speed: float = 500):
while True:
if Button.BEACON in self.ir_sensor.buttons(
channel=self.ir_beacon_channel):
if self.touch_sensor.pressed():
self.speaker.play_file(file=SoundFile.AIR_RELEASE)
self.grip_motor.run_time(speed=speed,
time=1000,
then=Stop.BRAKE,
wait=True)
else:
self.speaker.play_file(file=SoundFile.AIRBRAKE)
self.grip_motor.run(speed=-speed)
while not self.touch_sensor.pressed():
pass
self.grip_motor.stop()
while Button.BEACON in self.ir_sensor.buttons(
channel=self.ir_beacon_channel):
pass
def main(self, speed: float = 1000):
self.grip_motor.run_time(speed=-500,
time=1000,
then=Stop.BRAKE,
wait=True)
run_parallel(self.grip_or_release_by_ir_beacon,
self.keep_driving_by_ir_beacon)
class Spik3r(EV3Brick):
def __init__(
self,
sting_motor_port: Port = Port.D, go_motor_port: Port = Port.B,
claw_motor_port: Port = Port.A,
touch_sensor_port: Port = Port.S1, color_sensor_port: Port = Port.S3,
ir_sensor_port: Port = Port.S4, ir_beacon_channel: int = 1):
self.sting_motor = Motor(port=sting_motor_port,
positive_direction=Direction.CLOCKWISE)
self.go_motor = Motor(port=go_motor_port,
positive_direction=Direction.CLOCKWISE)
self.claw_motor = Motor(port=claw_motor_port,
positive_direction=Direction.CLOCKWISE)
self.ir_sensor = InfraredSensor(port=ir_sensor_port)
self.ir_beacon_channel = ir_beacon_channel
self.touch_sensor = TouchSensor(port=touch_sensor_port)
self.color_sensor = ColorSensor(port=color_sensor_port)
def sting_by_ir_beacon(self):
while True:
ir_buttons_pressed = set(self.ir_sensor.buttons(channel=self.ir_beacon_channel))
if ir_buttons_pressed == {Button.BEACON}:
self.sting_motor.run_angle(
speed=-750,
rotation_angle=220,
then=Stop.HOLD,
wait=False)
self.speaker.play_file(file=SoundFile.EV3)
self.sting_motor.run_time(
speed=-1000,
time=1000,
then=Stop.HOLD,
wait=True)
self.sting_motor.run_time(
speed=1000,
time=1000,
then=Stop.HOLD,
wait=True)
while Button.BEACON in self.ir_sensor.buttons(channel=self.ir_beacon_channel):
pass
def be_noisy_to_people(self):
while True:
if self.color_sensor.reflection() > 30:
for i in range(4):
self.speaker.play_file(file=SoundFile.ERROR_ALARM)
def pinch_if_touched(self):
while True:
if self.touch_sensor.pressed():
self.claw_motor.run_time(
speed=500,
time=1000,
then=Stop.HOLD,
wait=True)
self.claw_motor.run_time(
speed=-500,
time=0.3 * 1000,
then=Stop.HOLD,
wait=True)
def keep_driving_by_ir_beacon(self):
while True:
ir_buttons_pressed = set(self.ir_sensor.buttons(channel=self.ir_beacon_channel))
if ir_buttons_pressed == {Button.RIGHT_UP, Button.LEFT_UP}:
self.go_motor.run(speed=910)
elif ir_buttons_pressed == {Button.RIGHT_UP}:
self.go_motor.run(speed=-1000)
else:
self.go_motor.stop()
def main(self):
self.screen.load_image(ImageFile.EVIL)
Thread(target=self.be_noisy_to_people).start()
Thread(target=self.sting_by_ir_beacon).start()
Thread(target=self.pinch_if_touched).start()
self.keep_driving_by_ir_beacon()
class Robot:
"""Classe do Robo"""
"""Todos os métodos tem como primeiro parâmetro a palavra-chave 'self' e é ela que faz a referência
a outras variáveis e métodos dentro da classe 'Robot'.
Os outros parametros funcionam da mesma forma que em funcoes normais.
"""
def __init__(self, portaMotorEsquerdo, portaMotorDireito):
"""Esse metodo é a função de inicialização de um novo dado do tipo 'Robot'.
Podemos dizer, então, que é o método de inicialização de um novo objeto da classe 'Robot'.
Passamos os parametros:
'self', por ser um método, e as portas que serão associadas aos elementos do robo:"""
self.motorEsquerdo = Motor(portaMotorEsquerdo)
self.motorDireito = Motor(portaMotorDireito)
self.sensor = None # Como nao sabemos qual tipo de sensor será conectado, vamos deixar a variável <sensor> sem nenhum valor por enquanto
self.tipoSensor = "nenhum" # Podemos usar essa string para verificar qual o tipo de sensor conectado antes de utilizar ele no codigo
""" Cada tipo de sensor diferente que pode ser conectado tem uma inicialização diferente
Por isso podemos definir uma inicialização para cada tipo que poderemos utilizar
Se soubessemos exatamente quais sensores estariam no robo, eles poderiam ser inicializados direto no metodo <__init__>,
assim como foram os motores.
Nesse caso, nao seria necessario verificar o tipo de sensor, pois sempre saberiamos qual o tipo e de que forma utiliza-lo."""
def iniciaSensorCor(self, portaSensor): # Para o sensor de cor
self.sensor = ColorSensor(portaSensor)
self.tipoSensor = "cor"
def iniciaSensorUltra(self, portaSensor): # Para o sensor ultrassonico
self.sensor = UltrasonicSensor(portaSensor)
self.tipoSensor = "ultra"
def iniciaSensorInfra(self, portaSensor): # Para o sensor infravermelho
self.sensor = InfraredSensor(portaSensor)
self.tipoSensor = "infra"
def iniciaSensorGiro(self, portaSensor): # Para o sensor giroscopio
self.sensor = GyroSensor(portaSensor)
self.tipoSensor = "giro"
"""Metodos para utilizacao dos recursos do robo:"""
def andarTempo(self, velocEsquerda, velocDireita, tempo):
cronometro = StopWatch() # Definimos um cronometro
cronometro.reset() # Resetamos o tempo marcado no cronometro
while cronometro.time() < tempo:
self.motorDireito.run(velocEsquerda)
self.motorEsquerdo.run(velocDireita)
self.motorDireito.stop()
self.motorEsquerdo.stop()
def andarRetoGraus(self, veloc, graus):
while (self.motorEsquerdo.angle() < graus) and (self.motorDireito.angle() < graus):
self.motorDireito.run(veloc)
self.motorEsquerdo.run(veloc)
self.motorDireito.stop()
self.motorEsquerdo.stop()
def curvaGiro(self, veloc, graus): # Curva com os dois motores utilizando o giroscopio
if self.tipoSensor != "giro": # Verifica se o sensor do tipo certo esta conectado
print("ERRO: GIROSCOPIO NAO CONECTADO.")
return False # Interrompe o metodo
self.sensor.reset_angle(0)
while self.sensor.angle() < graus:
self.motorDireito.run(-veloc)
self.motorEsquerdo.run(veloc)
self.motorDireito.stop()
self.motorEsquerdo.stop()
def andaAteObstaculo(self, veloc, distancia):
if self.tipoSensor != "ultra" and self.tipoSensor != "infra": # O mesmo codigo funciona pro ultrassonico e pro infravermelho
print("ERRO: SENSOR DE DISTANCIA NAO CONECTADO")
return False # Interrompe o metodo
while self.sensor.distance() < distancia:
self.motorEsquerdo.run(veloc)
self.motorDireito.run(veloc)
self.motorEsquerdo.stop()
self.motorDireito.stop()
def andaAteCor(self, veloc, cor):
if self.tipoSensor != "cor":
print("ERRO: SENSOR DE COR NAO CONECTADO")
return False # Interrompe o metodo
while self.sensor.color() != cor:
self.motorEsquerdo.run(veloc)
self.motorDireito.run(veloc)
self.motorEsquerdo.stop()
self.motorDireito.stop()
def angPoligono(self, lados):
if lados <= 2 :
print("NAO EH UM POLIGONO")
return False
if self.tipoSensor != "giro":
print("ERRO: SENSOR GIROSCOPIO NAO CONECTADO")
return False # Interrompe o metodo
angint = (((lados - 2)*180)/lados)
#calculo para angulos internos de um polígono"
return angint
class Pilot:
"""Pilot class allowing user to drive the robot"""
def __init__(self):
self.speed = 0 # speed of the robot in deg/s
self.dist_proportion = 1 # proportion of the speed put in the left motor
self.angleSpeed = 0 # speed to substract to one wheel to make the robot turn
self.turnItRight = 0 # determine the current angle speed, the higher it is, the more it turns to the right
self.turnItLeft = 0 # determine the current angle speed, the higher it is, the more it turns to the left
self.left_motor = Motor(Port.B)
self.right_motor = Motor(Port.C)
def forward(self, speed):
"""Make the robot go forward at 'speed' in deg/s"""
self.speed = speed
self.dist_proportion = 1
self.angleSpeed = 0
self.left_motor.run(speed)
self.right_motor.run(speed)
def forwardTurnRightLog(self, speedPercentage, angleAcc):
"""Make the robot turns right using a logaritmic function.
The longer the robot turns, the more it will turn right until the robot reaches MAX_ANGLE_SPEED.
This function use a logarimtic function to increase the angle speed.
speedPercentage: relative speed the robot should be using while turning
angleAcc: relative acceleration of the turn (from 0 to 1)
"""
self.dist_proportion = 1
self.speed = round(MAX_SPEED * (speedPercentage/100))
self.turnItLeft = 0
self.turnItRight = self.turnItRight + angleAcc
self.angleSpeed = min(MAX_ANGLE_SPEED, round(((math.log(self.turnItRight)+3)/4)*MAX_ANGLE_SPEED))
self.left_motor.run(self.speed)
self.right_motor.run(self.speed-self.angleSpeed)
return self
def forwardTurnLeftLog(self, speedPercentage, angleAcc):
"""Make the robot turns left using a logaritmic function.
The longer the robot turns, the more it will turn left until the robot reaches MAX_ANGLE_SPEED.
This function use a logarimtic function to increase the angle speed.
speedPercentage: relative speed the robot should be using while turning
angleAcc: relative acceleration of the turn (from 0 to 1)
"""
self.speed = round(MAX_SPEED * (speedPercentage/100))
self.dist_proportion = 1
self.turnItRight = 0
self.turnItLeft = self.turnItRight + angleAcc
self.angleSpeed = min(MAX_ANGLE_SPEED, round(((math.log(self.turnItRight)+3)/4)*MAX_ANGLE_SPEED))
self.left_motor.run(self.speed)
self.right_motor.run(self.speed-self.angleSpeed)
return self
def forwardTurnLeftExp(self, speedPercentage, angleAcc):
"""Make the robot turns left using a reverse exponential function.
The longer the robot turns, the more it will turn left until the robot reaches MAX_ANGLE_SPEED.
This function use a reverse exponential function to increase the angle speed.
speedPercentage: relative speed the robot should be using while turning
angleAcc: relative acceleration of the turn (from 0 to 1)
"""
self.speed = round(MAX_SPEED * (speedPercentage/100))
self.dist_proportion = 1
self.turnItRight = 0
self.turnItLeft = self.turnItRight + angleAcc
self.angleSpeed = min(MAX_ANGLE_SPEED, round((1/math.exp(self.turnItRight*(-2)))*MAX_ANGLE_SPEED))
self.left_motor.run(self.speed)
self.right_motor.run(self.speed-self.angleSpeed)
return self
def forwardTurnRightExp(self, speedPercentage, angleAcc):
"""Make the robot turns right using a reverse exponential function.
The longer the robot turns, the more it will turn right until the robot reaches MAX_ANGLE_SPEED.
This function use a reverse exponential function to increase the angle speed.
speedPercentage: relative speed the robot should be using while turning
angleAcc: relative acceleration of the turn (from 0 to 1)
"""
self.speed = round(MAX_SPEED * (speedPercentage/100))
self.dist_proportion = 1
self.turnItLeft = 0
self.turnItRight = self.turnItRight + angleAcc
self.angleSpeed = min(MAX_ANGLE_SPEED, round((1/math.exp(self.turnItRight*(-2)))*MAX_ANGLE_SPEED))
self.left_motor.run(self.speed)
self.right_motor.run(self.speed-self.angleSpeed)
return self
def forwardTurn2(self, speedPercentage, angle):
"""Make the robot turns making a curve
speedPercentage : the speed in percent of the robot
angle : (-100 to 100) an relative angle, the more it is high, the more the robot turn"""
self.speed = MAX_SPEED * (speedPercentage/100)
relativeAngle = (self.speed * angle) / 100
if(angle<0):
if(self.speed > 0 ):
self.dist_proportion = (self.speed+2*relativeAngle)/self.speed
self.left_motor.run(self.speed+2*relativeAngle)
self.right_motor.run(self.speed)
elif(angle>0):
self.dist_proportion = 1
self.left_motor.run(self.speed)
self.right_motor.run(self.speed-2*relativeAngle)
def forwardRelative(self, speedPercentage):
"""Make the robot move forward using relative speed (0 to 100)."""
self.speed = MAX_SPEED * (speedPercentage/100) #MAX_SPEED vitesse maximale du robot
self.angleSpeed = 0
self.dist_proportion = 1
self.left_motor.run(self.speed)
self.right_motor.run(self.speed)
def stop(self):
"""Make the robot stop moving by stoping all the motors."""
self.speed = 0
self.dist_proportion = 1
self.left_motor.stop()
self.right_motor.stop()
class Kraz33Hors3:
def __init__(self,
back_foot_motor_port: Port = Port.C,
front_foot_motor_port: Port = Port.B,
gear_motor_port: Port = Port.A,
touch_sensor_port: Port = Port.S1,
color_sensor_port: Port = Port.S3,
ir_sensor_port: Port = Port.S4,
ir_beacon_channel: int = 1):
self.front_foot_motor = Motor(port=front_foot_motor_port,
positive_direction=Direction.CLOCKWISE)
self.back_foot_motor = Motor(
port=back_foot_motor_port,
positive_direction=Direction.COUNTERCLOCKWISE)
self.gear_motor = Motor(port=gear_motor_port)
self.touch_sensor = TouchSensor(port=touch_sensor_port)
self.color_sensor = ColorSensor(port=color_sensor_port)
self.ir_sensor = InfraredSensor(port=ir_sensor_port)
self.ir_beacon_channel = ir_beacon_channel
def drive_once_by_ir_beacon(
self,
speed: float = 1000 # deg/s
):
ir_beacons_pressed = set(
self.ir_sensor.buttons(channel=self.ir_beacon_channel))
# forward
if ir_beacons_pressed == {Button.LEFT_UP, Button.RIGHT_UP}:
self.front_foot_motor.run_time(
speed=speed,
time=1000, # ms
then=Stop.COAST,
wait=False)
self.back_foot_motor.run_time(
speed=speed,
time=1000, # ms
then=Stop.COAST,
wait=True)
# backward
elif ir_beacons_pressed == {Button.LEFT_DOWN, Button.RIGHT_DOWN}:
self.front_foot_motor.run_time(
speed=-speed,
time=1000, # ms
then=Stop.COAST,
wait=False)
self.back_foot_motor.run_time(
speed=-speed,
time=1000, # ms
then=Stop.COAST,
wait=True)
# move crazily
elif ir_beacons_pressed == {Button.BEACON}:
self.gear_motor.run(speed=speed)
self.front_foot_motor.run_time(
speed=speed / 3,
time=1000, # ms
then=Stop.COAST,
wait=False)
self.back_foot_motor.run_time(
speed=-speed / 3,
time=1000, # ms
then=Stop.COAST,
wait=True)
else:
self.gear_motor.stop()
def keep_driving_by_ir_beacon(
self,
speed: float = 1000 # deg/s
):
while True:
self.drive_once_by_ir_beacon(speed=speed)
def back_whenever_touched(
self,
speed: float = 1000 # deg/s
):
while True:
if self.touch_sensor.pressed():
self.front_foot_motor.run_time(
speed=-speed,
time=1000, # ms
then=Stop.COAST,
wait=False)
self.back_foot_motor.run_time(
speed=-speed,
time=1000, # ms
then=Stop.COAST,
wait=True)
def main(
self,
speed: float = 1000 # deg/s
):
Process(target=self.back_whenever_touched).start()
# FIXME: as soon as Touch Sensor pressed
# OSError: [Errno 5] EIO:
# Unexpected hardware input/output error with a motor or sensor:
# --> Try unplugging the sensor or motor and plug it back in again.
# --> To see which sensor or motor is causing the problem,
# check the line in your script that matches
# the line number given in the 'Traceback' above.
# --> Try rebooting the hub/brick if the problem persists.
self.keep_driving_by_ir_beacon(speed=speed)
class Puppy:
# These constants are used for positioning the legs.
HALF_UP_ANGLE = 25
STAND_UP_ANGLE = 65
STRETCH_ANGLE = 125
# These constants are for positioning the head.
HEAD_UP_ANGLE = 0
HEAD_DOWN_ANGLE = -40
# These constants are for the eyes.
#replaced HURT, HEART, SQUINTY with AWAKE,DIZZY,PINCHED_MIDDLE respectively
NEUTRAL_EYES = Image(ImageFile.NEUTRAL)
TIRED_EYES = Image(ImageFile.TIRED_MIDDLE)
TIRED_LEFT_EYES = Image(ImageFile.TIRED_LEFT)
TIRED_RIGHT_EYES = Image(ImageFile.TIRED_RIGHT)
SLEEPING_EYES = Image(ImageFile.SLEEPING)
HURT_EYES = Image(ImageFile.AWAKE)
ANGRY_EYES = Image(ImageFile.ANGRY)
HEART_EYES = Image(ImageFile.DIZZY)
SQUINTY_EYES = Image(ImageFile.PINCHED_MIDDLE)
def __init__(self):
# Initialize the EV3 brick.
self.ev3 = EV3Brick()
# Initialize the motors connected to the back legs.
self.left_leg_motor = Motor(Port.D, Direction.COUNTERCLOCKWISE)
self.right_leg_motor = Motor(Port.A, Direction.COUNTERCLOCKWISE)
# Initialize the motor connected to the head.
# Worm gear moves 1 tooth per rotation. It is interfaced to a 24-tooth
# gear. The 24-tooth gear is connected to parallel 12-tooth gears via
# an axle. The 12-tooth gears interface with 36-tooth gears.
self.head_motor = Motor(Port.C,
Direction.COUNTERCLOCKWISE,
gears=[[1, 24], [12, 36]])
# Initialize the Color Sensor.
self.color_sensor = ColorSensor(Port.S4)
# Initialize the touch sensor.
self.touch_sensor = TouchSensor(Port.S1)
# This attribute is used by properties.
self._eyes = None
# These attributes are used by the playful behavior.
self.playful_timer = StopWatch()
self.playful_bark_interval = None
def adjust_head(self):
"""Use the up and down buttons on the EV3 brick to adjust the puppy's
head up or down.
"""
self.ev3.screen.show_image(ImageFile.EV3_ICON)
self.ev3.light.on(Color.ORANGE)
while True:
buttons = self.ev3.buttons.pressed()
if Button.CENTER in buttons:
break
elif Button.UP in buttons:
self.head_motor.run(20)
elif Button.DOWN in buttons:
self.head_motor.run(-20)
else:
self.head_motor.stop()
wait(100)
self.head_motor.stop()
self.head_motor.reset_angle(0)
self.ev3.light.on(Color.GREEN)
def move_head(self, target):
"""Move the head to the target angle.
Arguments:
target (int):
The target angle in degrees. 0 is the starting position,
negative values are below this point and positive values
are above this point.
"""
self.head_motor.run_target(20, target)
def reset(self):
# must be called when puppy is sitting down.
self.left_leg_motor.reset_angle(0)
self.right_leg_motor.reset_angle(0)
self.behavior = self.idle
# The next 10 methods define the 10 behaviors of the puppy.
def idle(self):
"""The puppy is idle."""
self.stand_up()
self.eyes = self.NEUTRAL_EYES
def go_to_sleep(self):
"""Makes the puppy go to sleep.
Press the center button and touch sensor at the same time to awaken the puppy."""
self.eyes = self.TIRED_EYES
self.sit_down()
self.move_head(self.HEAD_DOWN_ANGLE)
self.eyes = self.SLEEPING_EYES
self.ev3.speaker.play_file(SoundFile.SNORING)
if self.touch_sensor.pressed(
) and Button.CENTER in self.ev3.buttons.pressed():
self.behavior = self.wake_up
def wake_up(self):
"""Makes the puppy wake up."""
self.eyes = self.TIRED_EYES
self.ev3.speaker.play_file(SoundFile.DOG_WHINE)
self.move_head(self.HEAD_UP_ANGLE)
self.sit_down()
self.stretch()
wait(1000)
self.stand_up()
self.behavior = self.idle
def act_playful(self):
"""Makes the puppy act playful."""
self.eyes = self.NEUTRAL_EYES
self.stand_up()
self.playful_bark_interval = 0
if self.playful_timer.time() > self.playful_bark_interval:
self.ev3.speaker.play_file(SoundFile.DOG_BARK_2)
self.playful_timer.reset()
self.playful_bark_interval = randint(4, 8) * 1000
def act_angry(self):
"""Makes the puppy act angry."""
self.eyes = self.ANGRY_EYES
self.ev3.speaker.play_file(SoundFile.DOG_GROWL)
self.stand_up()
wait(1500)
self.ev3.speaker.play_file(SoundFile.DOG_BARK_1)
def act_hungry(self):
"""Makes the puppy act hungry."""
self.eyes = self.HURT_EYES
self.sit_down()
self.ev3.speaker.play_file(SoundFile.DOG_WHINE)
def go_to_bathroom(self):
"""Makes the puppy go to the bathroom."""
self.eyes = self.SQUINTY_EYES
self.stand_up()
wait(100)
self.right_leg_motor.run_target(100, self.STRETCH_ANGLE)
wait(800)
self.ev3.speaker.play_file(SoundFile.HORN_1)
wait(1000)
for _ in range(3):
self.right_leg_motor.run_angle(100, 20)
self.right_leg_motor.run_angle(100, -20)
self.right_leg_motor.run_target(100, self.STAND_UP_ANGLE)
self.behavior = self.idle
def act_happy(self):
"""Makes the puppy act happy."""
self.eyes = self.HEART_EYES
# self.move_head(self.?)
self.sit_down()
for _ in range(3):
self.ev3.speaker.play_file(SoundFile.DOG_BARK_1)
self.hop()
wait(500)
self.sit_down()
self.reset()
def sit_down(self):
"""Makes the puppy sit down."""
self.left_leg_motor.run(-50)
self.right_leg_motor.run(-50)
wait(1000)
self.left_leg_motor.stop()
self.right_leg_motor.stop()
wait(100)
def walk_steps(self):
"""Makes the puppy walk a certain number of steps.
Modify front wheels to roll by removing anchoring pegs and switching pegs through the axle to non-friction pegs.
Change steps to adjuct the number of steps."""
#steps equals number of steps pup takes
steps = 10
self.stand_up()
for value in range(1, steps + 1):
self.left_leg_motor.run_target(-100, 25, wait=False)
self.right_leg_motor.run_target(-100, 25)
while not self.left_leg_motor.control.target_tolerances():
wait(200)
self.left_leg_motor.run_target(100, 65, wait=False)
self.right_leg_motor.run_target(100, 65)
while not self.left_leg_motor.control.target_tolerances():
wait(200)
self.left_leg_motor.run_target(50, 65, wait=False)
self.right_leg_motor.run_target(50, 65)
wait(100)
def walk_timed(self):
"""Makes the puppy walk a certain time in ms.
Modify front wheels to roll by removing anchoring pegs and switching pegs through the axle to non-friction pegs.
Change walk_time to adjust the time the puppy walks in ms."""
#walk_time equals desired walk time in ms
walk_time = 6000
elapsed_time = StopWatch()
while elapsed_time.time() < walk_time:
self.left_leg_motor.run_target(-100, 25, wait=False)
self.right_leg_motor.run_target(-100, 25)
while not self.left_leg_motor.control.target_tolerances():
wait(200)
self.left_leg_motor.run_target(100, 65, wait=False)
self.right_leg_motor.run_target(100, 65)
while not self.left_leg_motor.control.target_tolerances():
wait(200)
self.left_leg_motor.run_target(50, 65, wait=False)
self.right_leg_motor.run_target(50, 65)
wait(100)
elapsed_time.reset()
# The next 4 methods define actions that are used to make some parts of
# the behaviors above.
def stand_up(self):
"""Makes the puppy stand up."""
self.left_leg_motor.run_target(100, self.HALF_UP_ANGLE, wait=False)
self.right_leg_motor.run_target(100, self.HALF_UP_ANGLE)
while not self.left_leg_motor.control.target_tolerances():
wait(100)
self.left_leg_motor.run_target(50, self.STAND_UP_ANGLE, wait=False)
self.right_leg_motor.run_target(50, self.STAND_UP_ANGLE)
while not self.left_leg_motor.control.target_tolerances():
wait(100)
wait(500)
def stretch(self):
"""Makes the puppy stretch its legs backwards."""
self.stand_up()
self.left_leg_motor.run_target(100, self.STRETCH_ANGLE, wait=False)
self.right_leg_motor.run_target(100, self.STRETCH_ANGLE)
self.ev3.speaker.play_file(SoundFile.DOG_WHINE)
self.left_leg_motor.run_target(100, self.STAND_UP_ANGLE, wait=False)
self.right_leg_motor.run_target(100, self.STAND_UP_ANGLE)
wait(500)
def hop(self):
"""Makes the puppy hop."""
self.left_leg_motor.run(500)
self.right_leg_motor.run(500)
wait(275)
self.left_leg_motor.stop()
self.right_leg_motor.stop()
wait(275)
self.left_leg_motor.run(-50)
self.right_leg_motor.run(-50)
wait(275)
self.left_leg_motor.stop()
self.right_leg_motor.stop()
@property
def eyes(self):
"""Gets and sets the eyes."""
return self._eyes
@eyes.setter
def eyes(self, value):
if value != self._eyes:
self._eyes = value
self.ev3.screen.show_image(value)
def run(self):
"""This is the main program run loop."""
self.sit_down()
self.adjust_head()
self.eyes = self.SLEEPING_EYES
self.reset()
#self.eyes = self.SLEEPING_EYES
"""The following code cycles through all of the behaviors, separated by beeps."""
self.act_playful()
wait(1000)
self.ev3.speaker.beep()
self.act_happy()
wait(1000)
self.ev3.speaker.beep()
self.act_hungry()
wait(1000)
self.ev3.speaker.beep()
self.act_angry()
wait(1000)
self.ev3.speaker.beep()
self.go_to_bathroom()
wait(1000)
self.ev3.speaker.beep()
self.go_to_sleep()
wait(1000)
self.ev3.speaker.beep()
self.wake_up()
wait(1000)
self.ev3.speaker.beep()
self.walk_steps()
wait(1000)
self.ev3.speaker.beep()
self.walk_timed()
wait(1000)
self.ev3.speaker.beep()
self.idle()
wait(1000)
self.ev3.speaker.beep()
class Gripp3r(EV3Brick):
WHEEL_DIAMETER = 26
AXLE_TRACK = 115
def __init__(self,
left_motor_port: Port = Port.B,
right_motor_port: Port = Port.C,
grip_motor_port: Port = Port.A,
touch_sensor_port: Port = Port.S1,
ir_sensor_port: Port = Port.S4,
ir_beacon_channel: int = 1):
self.drive_base = DriveBase(
left_motor=Motor(port=left_motor_port,
positive_direction=Direction.CLOCKWISE),
right_motor=Motor(port=right_motor_port,
positive_direction=Direction.CLOCKWISE),
wheel_diameter=self.WHEEL_DIAMETER,
axle_track=self.AXLE_TRACK)
self.drive_base.settings(
straight_speed=750, # milimeters per second
straight_acceleration=750,
turn_rate=90, # degrees per second
turn_acceleration=90)
self.grip_motor = Motor(port=grip_motor_port,
positive_direction=Direction.CLOCKWISE)
self.touch_sensor = TouchSensor(port=touch_sensor_port)
self.ir_sensor = InfraredSensor(port=ir_sensor_port)
self.ir_beacon_channel = ir_beacon_channel
def keep_driving_by_ir_beacon(
self,
channel: int = 1,
speed: float = 1000 # milimeters per second
):
while True:
ir_beacon_buttons_pressed = set(
self.ir_sensor.buttons(channel=channel))
# forward
if ir_beacon_buttons_pressed == {Button.LEFT_UP, Button.RIGHT_UP}:
self.drive_base.drive(
speed=speed,
turn_rate=0 # degrees per second
)
# backward
elif ir_beacon_buttons_pressed == {
Button.LEFT_DOWN, Button.RIGHT_DOWN
}:
self.drive_base.drive(
speed=-speed,
turn_rate=0 # degrees per second
)
# turn left on the spot
elif ir_beacon_buttons_pressed == {
Button.LEFT_UP, Button.RIGHT_DOWN
}:
self.drive_base.drive(
speed=0,
turn_rate=-90 # degrees per second
)
# turn right on the spot
elif ir_beacon_buttons_pressed == {
Button.LEFT_DOWN, Button.RIGHT_UP
}:
self.drive_base.drive(
speed=0,
turn_rate=90 # degrees per second
)
# turn left forward
elif ir_beacon_buttons_pressed == {Button.LEFT_UP}:
self.drive_base.drive(
speed=speed,
turn_rate=-90 # degrees per second
)
# turn right forward
elif ir_beacon_buttons_pressed == {Button.RIGHT_UP}:
self.drive_base.drive(
speed=speed,
turn_rate=90 # degrees per second
)
# turn left backward
elif ir_beacon_buttons_pressed == {Button.LEFT_DOWN}:
self.drive_base.drive(
speed=-speed,
turn_rate=90 # degrees per second
)
# turn right backward
elif ir_beacon_buttons_pressed == {Button.RIGHT_DOWN}:
self.drive_base.drive(
speed=-speed,
turn_rate=-90 # degrees per second
)
# otherwise stop
else:
self.drive_base.stop()
def grip_or_release_by_ir_beacon(self, speed: float = 500):
while True:
if Button.BEACON in self.ir_sensor.buttons(
channel=self.ir_beacon_channel):
if self.touch_sensor.pressed():
self.speaker.play_file(file=SoundFile.AIR_RELEASE)
self.grip_motor.run_time(speed=speed,
time=1000,
then=Stop.BRAKE,
wait=True)
else:
self.speaker.play_file(file=SoundFile.AIRBRAKE)
self.grip_motor.run(speed=-speed)
while not self.touch_sensor.pressed():
pass
self.grip_motor.stop()
while Button.BEACON in self.ir_sensor.buttons(
channel=self.ir_beacon_channel):
pass
def main(self, speed: float = 1000):
self.grip_motor.run_time(speed=-500,
time=1000,
then=Stop.BRAKE,
wait=True)
Process(target=self.grip_or_release_by_ir_beacon).start()
self.keep_driving_by_ir_beacon(speed=speed)
BRICK = EV3Brick()
MEDIUM_MOTOR = Motor(port=Port.A, positive_direction=Direction.CLOCKWISE)
TAIL_MOTOR = Motor(port=Port.B, positive_direction=Direction.CLOCKWISE)
CHEST_MOTOR = Motor(port=Port.D, positive_direction=Direction.CLOCKWISE)
IR_SENSOR = InfraredSensor(port=Port.S4)
CHEST_MOTOR.run_time(speed=-300, time=1000, then=Stop.HOLD, wait=True)
while True:
if IR_SENSOR.distance() < 30:
BRICK.light.on(color=Color.RED)
MEDIUM_MOTOR.stop()
TAIL_MOTOR.stop()
BRICK.speaker.play_file(file=SoundFile.SNAKE_HISS)
CHEST_MOTOR.run_time(speed=1000, time=1000, then=Stop.HOLD, wait=True)
MEDIUM_MOTOR.run(speed=1000)
TAIL_MOTOR.run(speed=-1000)
CHEST_MOTOR.run_time(speed=-300, time=1000, then=Stop.HOLD, wait=True)
sleep(2)
