def waehle(self, str_sensor):
"""
choice of sensor
"""
headmotor = Motor(Port.A, Direction.COUNTERCLOCKWISE)
angle_soll = self.dict_head[str_sensor]
angle_diff = angle_soll - self.angle_ist
debug('angle ' + str(angle_diff))
headmotor.run_target(speed=20, target_angle=angle_diff)
self.angle_ist = angle_soll
self._schreibe_winkel()
def __init__(self, arg_correction=None):
self.dict_head = {'ir': 0, 'touch': -110, 'color': 90}
self.datei = 'winkel.txt'
self.angle_ist = 0
if arg_correction is not None:
headmotor = Motor(Port.A, Direction.COUNTERCLOCKWISE)
if type(arg_correction) == str:
angle = -self.dict_head[arg_correction]
else:
angle = arg_correction
headmotor.run_target(speed=20, target_angle=angle)
else:
if self.datei in os.listdir(): # gibt es die Datei?
debug('lese')
self._lese_winkel()
debug('init ' + str(self.angle_ist))
def kalibriere(self):
headmotor = Motor(Port.A, Direction.COUNTERCLOCKWISE)
farbsensor = ColorSensor(Port.S3)
headmotor.run_until_stalled(speed=10, duty_limit=50)
debug('winkel=' + str(headmotor.angle()))
headmotor.run_target(speed=10, target_angle=-120, wait=False)
while (farbsensor.reflection() < 10): # & (headmotor.speed() != 0):
debug('farbwert=' + str(farbsensor.reflection()))
time.sleep(0.1)
headmotor.stop()
headmotor.run_angle(speed=10, rotation_angle=15)
debug(str(farbsensor.reflection()))
# winkel auf 0
headmotor.reset_angle(0)
self.angle_ist = 0
self._schreibe_winkel()
class SmallMotor:
def __init__(self, ev3, port):
self.ev3 = ev3
self.motor = Motor(port, Direction.COUNTERCLOCKWISE)
self.motor.reset_angle(0)
def reset(self):
self.motor.run_until_stalled(100)
self.motor.run_angle(800, -300)
self.motor.reset_angle(0)
def moveTo(self, angle, speed = 800, wait = False):
print(self.motor.angle())
self.motor.run_target(speed, angle, Stop.HOLD, wait)
print(self.motor.angle())
def move(self, speed = 20):
self.motor.run(speed)
def brake(self):
self.motor.brake()
#!/usr/bin/env pybricks-micropython from pybricks import ev3brick as brick from pybricks.ev3devices import Motor from pybricks.parameters import Port left_motor = Motor(Port.B) left_motor.run_angle(360, 360, Stop.BRAKE, True) wait(1000) left_motor.run_angle(180, 180, Stop.BRAKE, True) wait(1000) left_motor.run_target(360, 0, Stop.BRAKE, True) wait(1000)
ev3.screen.print('=run hold=')
motorB.run_time(speed=500, time=3000, then=Stop.HOLD, wait=True)
waiter(ir)
ev3.screen.print('=reset angle 0=')
motorB.reset_angle(0)
printMotor(motorB, ev3.screen)
waiter(ir)
ev3.screen.print('=angle 180=')
motorB.run_angle(speed=500, rotation_angle=180, then=Stop.HOLD, wait=True)
printMotor(motorB, ev3.screen)
waiter(ir)
ev3.screen.print('=run target 90=')
motorB.run_target(speed=500, target_angle=90, then=Stop.HOLD, wait=True)
printMotor(motorB, ev3.screen)
waiter(ir)
ev3.screen.print('=angle 90=')
motorB.run_angle(speed=500, rotation_angle=90, then=Stop.HOLD, wait=True)
printMotor(motorB, ev3.screen)
waiter(ir)
ev3.screen.print('=track target 90=')
motorB.track_target(target_angle=90)
waiter(ir)
ev3.screen.print('=until stalled=')
motorB.run_until_stalled(15, then=Stop.COAST, duty_limit=10)
printMotor(motorB, ev3.screen)
D = 293.665
C = 261.626
lowB = 246.942
#Hz = [261.626, 261.626, 391.995, 391.995, 440.000, 440.000, 391.995], d = [500, 500, 500, 500, 500, 500, 1000]
def sing(Hz=[
G, G, Fsharp, Fsharp, lowB, D, lowB, lowB, G, G, Fsharp, Fsharp, lowB, 1,
G, G, Fsharp, Fsharp, lowB, D, lowB, D, E, C, D, lowB, 1
],
d=[
1, 1, 1, 1, 1, 0.5, 1.5, 1, 0.5, 1.5, 1, 1, 1, 3, 1, 1, 1, 1, 1,
1, 2, 1, 1, 1, 1, 1, 3
]):
#Sings the song entered. Default is Spooky Scary Skeletons
for f in range(0, len(Hz)):
if Hz[f] == 1:
wait(250 * d[f])
else:
brick.sound.beep(Hz[f], 250 * d[f])
wait(125)
while True:
if sensor1.distance() < 1000:
sing()
for a in range(0, 4):
motorB.run_target(500, 360)
motorB.run_target(500, 0)
class SpikeManager:
def __init__(self):
# Initialize all devices
self.ev3 = EV3Brick()
self.usb_motor = Motor(Port.D)
self.bt_motor = Motor(Port.C)
self.left_button_motor = Motor(Port.B)
self.right_button_motor = Motor(Port.A)
# Reset all motor to mechanical stop
self.usb_motor.run_until_stalled(-SPEED, duty_limit=50)
self.bt_motor.run_until_stalled(-SPEED, duty_limit=20)
self.left_button_motor.run_until_stalled(-SPEED, duty_limit=100)
self.right_button_motor.run_until_stalled(SPEED, duty_limit=30)
wait(500)
# Reset the angles
self.usb_motor.reset_angle(10)
self.bt_motor.reset_angle(-20)
self.left_button_motor.reset_angle(-25)
self.right_button_motor.reset_angle(20)
# Go to neutral position
self.reset()
def reset(self):
self.usb_motor.run_target(SPEED, 0)
self.bt_motor.run_target(SPEED, 0)
self.left_button_motor.run_target(SPEED, 0)
self.right_button_motor.run_target(SPEED, 0)
def insert_usb(self):
self.usb_motor.run_target(SPEED, 70, then=Stop.COAST)
def remove_usb(self):
self.usb_motor.run_target(SPEED, 0, then=Stop.COAST)
def activate_dfu(self):
self.bt_motor.dc(-40)
wait(600)
self.insert_usb()
wait(8000)
self.bt_motor.run_target(SPEED, 0)
def shutdown(self):
self.left_button_motor.run_target(SPEED, 20)
wait(4000)
self.left_button_motor.run_target(SPEED, 0)
#!/usr/bin/env pybricks-micropython from pybricks import ev3brick as brick from pybricks.ev3devices import Motor from pybricks.parameters import Port # Play a sound. brick.sound.beep() # Initialize a motor at port B. test_motor = Motor(Port.B) # Run the motor up to 500 degrees per second. To a target angle of 90 degrees. test_motor.run_target(500, 90) # Play another beep sound. # This time with a higher pitch (1000 Hz) and longer duration (500 ms). brick.sound.beep(1000, 500)
#!/usr/bin/env pybricks-micropython from pybricks import ev3brick as brick from pybricks.ev3devices import Motor from pybricks.parameters import Port # ______first_bend________________________ test_motor = Motor(Port.A) # initialize a motor at port A test_motor.run_target(200, 50) # Run motor 200 degs/sec to target angle of 50° test_motor = Motor(Port.B) test_motor.run_target(200, 50) test_motor = Motor(Port.C) test_motor.run_target(200, 50) test_motor = Motor(Port.D) test_motor.run_target(200, 50) #_______second_bend_______________________ test_motor = Motor(Port.A) test_motor.run_target(200, -50) test_motor = Motor(Port.B) test_motor.run_target(200, -50) test_motor = Motor(Port.C) test_motor.run_target(200, -50) test_motor = Motor(Port.D)
global sound, manualSound
if sound == False:
if Button.LEFT_UP in infraredSensor.buttons(2):
switchSound()
else:
if Button.LEFT_DOWN in infraredSensor.buttons(2):
switchSound()
if Button.RIGHT_DOWN in infraredSensor.buttons(2):
manualSound = False
# system setup
ev3.speaker.set_volume(100, which='_all_')
motor.reset_angle(20)
soundMotor.reset_angle(0)
motor.run_target(500, 75, wait=True)
ev3.light.on(Color.GREEN)
watch.reset()
# main project loop
while shutdown == False:
''' finishing the progamm '''
if Button.LEFT_UP in infraredSensor.buttons(4):
shutdown = True
''' checking touch sensor '''
if touchSensor.pressed() == True:
if manualLight == False and manualSound == False:
manualLight = True
manualSound = True
break
wait(10)
lift_motor.hold()
# Move the robot forward for some time using the front and rear
# motors.
front_motor.dc(60)
rear_motor.dc(100)
wait(1300)
# Play a sound and pull the rear structure up so it gets back to
# its starting position. Keep moving forward slowly by
# simultaneously running the front and rear motors.
ev3.speaker.play_file(SoundFile.AIR_RELEASE)
front_motor.dc(30)
rear_motor.dc(30)
lift_motor.run_target(160, 0)
# Update the "steps" variable and display it on the screen.
steps -= 1
ev3.screen.clear()
ev3.screen.draw_text(70, 50, steps)
# Settle the robot at the top of a step and end the program.
front_motor.dc(100)
rear_motor.dc(90)
wait(2000)
front_motor.hold()
rear_motor.hold()
wait(5000)
WAIT_TRAIN = 50 # wait for Train (4DBrix) to receive command (empyrical)
INITSpeed = 320 # initial speed of our Train (you choose)
MAXSpeed = 1023 # maximum speed accepted by 4DBrix WiFi Controller
MOTORSpeed = 1024 # speed to use with EV3 motor (360 = 1 turn/sec)
TOLERANCE = 4 # acceptable tolerance when reading motor position
SETTLE = 250 # time (ticks) for the user to change motor position before
# considering it done (empyrical)
us = UltrasonicSensor(Port.S1)
m = Motor(Port.A)
m.reset_angle(0)
print('Motor Reset')
# motor will be used to read speed so defining scale
SCALE = round(360 / MAXSpeed, 2)
m.run_target(MOTORSpeed, round(INITSpeed * SCALE),
Stop.COAST) # show initial speed
# Possible hostnames - use your own
RIGHTMOST = 'iota'
LEFTMOST = 'alpha'
# get hostname to use as MQTT_ClientID and decide roles
# LEFTMOST should be at LEFT or F extreme
# RIGHTMOST should be at RIGHT or B extreme
os.system('hostname > /dev/shm/hostname.txt')
file = open('/dev/shm/hostname.txt', 'r')
MQTT_ClientID = file.readline().rstrip('\n')
file.close()
os.system('rm /dev/shm/hostname.txt')
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.
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.HURT)
ANGRY_EYES = Image(ImageFile.ANGRY)
HEART_EYES = Image(ImageFile.LOVE)
SQUINTY_EYES = Image(ImageFile.TEAR) # the tear is erased later
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. It is used to detect the colors when
# feeding the puppy.
self.color_sensor = ColorSensor(Port.S4)
# Initialize the touch sensor. It is used to detect when someone pets
# the puppy.
self.touch_sensor = TouchSensor(Port.S1)
self.pet_count_timer = StopWatch()
self.feed_count_timer = StopWatch()
self.count_changed_timer = StopWatch()
# These attributes are initialized later in the reset() method.
self.pet_target = None
self.feed_target = None
self.pet_count = None
self.feed_count = None
# These attributes are used by properties.
self._behavior = None
self._behavior_changed = None
self._eyes = None
self._eyes_changed = None
# These attributes are used in the eyes update
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
# These attributes are used by the playful behavior.
self.playful_timer = StopWatch()
self.playful_bark_interval = None
# These attributes are used in the update methods.
self.prev_petted = None
self.prev_color = 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.load_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)
# Pick a random number of time to pet the puppy.
self.pet_target = urandom.randint(3, 6)
# Pick a random number of time to feed the puppy.
self.feed_target = urandom.randint(2, 4)
# Pet count and feed count both start at 1
self.pet_count, self.feed_count = 1, 1
# Reset timers.
self.pet_count_timer.reset()
self.feed_count_timer.reset()
self.count_changed_timer.reset()
# Set initial behavior.
self.behavior = self.idle
# The next 8 methods define the 8 behaviors of the puppy.
def idle(self):
"""The puppy is idle and waiting for someone to pet it or feed it."""
if self.did_behavior_change:
print('idle')
self.stand_up()
self.update_eyes()
self.update_behavior()
self.update_pet_count()
self.update_feed_count()
def go_to_sleep(self):
"""Makes the puppy go to sleep."""
if self.did_behavior_change:
print('go_to_sleep')
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.count_changed_timer.reset()
self.behavior = self.wake_up
def wake_up(self):
"""Makes the puppy wake up."""
if self.did_behavior_change:
print('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."""
if self.did_behavior_change:
print('act_playful')
self.eyes = self.NEUTRAL_EYES
self.stand_up()
self.playful_bark_interval = 0
if self.update_pet_count():
# If the puppy was petted, then we are done being playful
self.behavior = self.idle
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 = urandom.randint(4, 8) * 1000
def act_angry(self):
"""Makes the puppy act angry."""
if self.did_behavior_change:
print('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)
self.pet_count -= 1
print('pet_count:', self.pet_count, 'pet_target:', self.pet_target)
self.behavior = self.idle
def act_hungry(self):
if self.did_behavior_change:
print('act_hungry')
self.eyes = self.HURT_EYES
self.sit_down()
self.ev3.speaker.play_file(SoundFile.DOG_WHINE)
if self.update_feed_count():
# If we got food, then we are not longer hungry.
self.behavior = self.idle
if self.update_pet_count():
# If we got a pet instead of food, then we are angry.
self.behavior = self.act_angry
def go_to_bathroom(self):
if self.did_behavior_change:
print('go_to_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.feed_count = 1
self.behavior = self.idle
def act_happy(self):
if self.did_behavior_change:
print('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)
# 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.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 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)
while not self.left_leg_motor.control.done():
wait(100)
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)
while not self.left_leg_motor.control.done():
wait(100)
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.hold()
self.right_leg_motor.hold()
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 behavior(self):
"""Gets and sets the current behavior."""
return self._behavior
@behavior.setter
def behavior(self, value):
if self._behavior != value:
self._behavior = value
self._behavior_changed = True
@property
def did_behavior_change(self):
"""bool: Tests if the behavior changed since the last time this
property was read.
"""
if self._behavior_changed:
self._behavior_changed = False
return True
return False
def update_behavior(self):
"""Updates the :prop:`behavior` property based on the current state
of petting and feeding.
"""
if self.pet_count == self.pet_target and self.feed_count == self.feed_target:
# If we have the exact right amount of pets and feeds, act happy.
self.behavior = self.act_happy
elif self.pet_count > self.pet_target and self.feed_count < self.feed_target:
# If we have too many pets and not enough food, act angry.
self.behavior = self.act_angry
elif self.pet_count < self.pet_target and self.feed_count > self.feed_target:
# If we have not enough pets and too much food, go to the bathroom.
self.behavior = self.go_to_bathroom
elif self.pet_count == 0 and self.feed_count > 0:
# If we have no pets and some food, act playful.
self.behavior = self.act_playful
elif self.feed_count == 0:
# If we have no food, act hungry.
self.behavior = self.act_hungry
@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.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 update_pet_count(self):
"""Updates the :attr:`pet_count` attribute if the puppy is currently
being petted (touch sensor pressed).
Returns:
bool:
``True`` if the puppy was petted since the last time this method
was called, otherwise ``False``.
"""
changed = False
petted = self.touch_sensor.pressed()
if petted and petted != self.prev_petted:
self.pet_count += 1
print('pet_count:', self.pet_count, 'pet_target:', self.pet_target)
self.count_changed_timer.reset()
if not self.behavior == self.act_hungry:
self.eyes = self.SQUINTY_EYES
self.ev3.speaker.play_file(SoundFile.DOG_SNIFF)
changed = True
self.prev_petted = petted
return changed
def update_feed_count(self):
"""Updates the :attr:`feed_count` attribute if the puppy is currently
being fed (color sensor detects a color).
Returns:
bool:
``True`` if the puppy was fed since the last time this method
was called, otherwise ``False``.
"""
color = self.color_sensor.color()
changed = False
if color is not None and color != Color.BLACK and color != self.prev_color:
self.feed_count += 1
print('feed_count:', self.feed_count, 'feed_target:',
self.feed_target)
changed = True
self.count_changed_timer.reset()
self.prev_color = color
self.eyes = self.SQUINTY_EYES
self.ev3.speaker.play_file(SoundFile.CRUNCHING)
return changed
def monitor_counts(self):
"""Monitors pet and feed counts and decreases them over time."""
if self.pet_count_timer.time() > 15000:
self.pet_count_timer.reset()
self.pet_count = max(0, self.pet_count - 1)
print('pet_count:', self.pet_count, 'pet_target:', self.pet_target)
if self.feed_count_timer.time() > 15000:
self.feed_count_timer.reset()
self.feed_count = max(0, self.feed_count - 1)
print('feed_count:', self.feed_count, 'feed_target:',
self.feed_target)
if self.count_changed_timer.time() > 30000:
# If nothing has happened for 30 seconds, go to sleep
self.count_changed_timer.reset()
self.behavior = self.go_to_sleep
def run(self):
"""This is the main program run loop."""
self.sit_down()
self.adjust_head()
self.eyes = self.SLEEPING_EYES
self.reset()
while True:
self.monitor_counts()
self.behavior()
wait(100)
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
# Defining variables
hand_motor = Motor(Port.A)
dispenser_motor = Motor(Port.B)
nose_motor = Motor(Port.C)
button = TouchSensor(Port.S2)
spider_eye = UltrasonicSensor(Port.S1)
degree = 0
#Moving arm to initial position
hand_motor.run_target(500, 90)
brick.light(Color.RED)
#Infinite loop
while True:
distance = spider_eye.distance(False)
#If something approaches sensor
if distance <= 2000:
#Arm moves
hand_motor.run_target(500, -90)
wait(500)
hand_motor.run_target(500, 90)
wait(500)
#Nose spins
nose_motor.run_target(500, 360)
brick.sound.file(SoundFile.LAUGHING_1)
class Charlie():
'''
Charlie is the class responsible for driving,
Robot-Movement and general Real-world interaction of the robot with Sensors and motors.
Args:
config (dict): The parsed config
brick (EV3Brick): EV3Brick for getting button input
logger (Logger): Logger for logging
'''
def __init__(self, config, brick, logger):
logger.info(self, 'Starting initialisation of Charlie')
self.__config = config
self.brick = brick
self.logger = logger
self.__conf2port = {
1: Port.S1,
2: Port.S2,
3: Port.S3,
4: Port.S4,
'A': Port.A,
'B': Port.B,
'C': Port.C,
'D': Port.D
}
self.__initSensors()
self.__initMotors()
self.min_speed = 35 # lage motor 20, medium motor 30
self.__gyro.reset_angle(0)
self.__screenRoutine = False
self.showDetails()
self.logger.info(self, 'Driving for Charlie initialized')
##TODO
def __repr__(self):
outputString = "(TODO)\n Config: " + self.__config + "\n Brick: " + self.brick + "\n Logger: " + self.logger
outputString += "\n--Debug--\n Minimum Speed: " + str(
self.min_speed) + "\n "
return "TODO"
def __str__(self):
return "Charlie"
def __initSensors(self):
'''Sub-method for initializing Sensors.'''
self.logger.debug(self, "Starting sensor initialisation...")
try:
self.__gyro = GyroSensor(
self.__conf2port[self.__config['gyroSensorPort']],
Direction.CLOCKWISE if not self.__config['gyroInverted'] else
Direction.COUNTERCLOCKWISE
) if self.__config['gyroSensorPort'] != 0 else 0
self.logger.debug(
self, 'Gyrosensor initialized sucessfully on port %s' %
self.__config['gyroSensorPort'])
except Exception as exception:
self.__gyro = 0
self.logger.error(
self,
"Failed to initialize the Gyro-Sensor - Are u sure it's connected to Port %s?"
% exception, exception)
try:
self.__rLight = ColorSensor(
self.__conf2port[self.__config['rightLightSensorPort']]
) if self.__config['rightLightSensorPort'] != 0 else 0
self.logger.debug(
self, 'Colorsensor initialized sucessfully on port %s' %
self.__config['rightLightSensorPort'])
except Exception as exception:
self.logger.error(
self,
"Failed to initialize the right Color-Sensor - Are u sure it's connected to Port %s?"
% exception, exception)
try:
self.__lLight = ColorSensor(
self.__conf2port[self.__config['leftLightSensorPort']]
) if self.__config['leftLightSensorPort'] != 0 else 0
self.logger.debug(
self, 'Colorsensor initialized sucessfully on port %s' %
self.__config['leftLightSensorPort'])
except Exception as exception:
self.logger.error(
self,
"Failed to initialize the left Color-Sensor - Are u sure it's connected to Port %s?"
% exception, exception)
try:
self.__touch = TouchSensor(
self.__conf2port[self.__config['backTouchSensor']]
) if self.__config['backTouchSensor'] != 0 else 0
self.logger.debug(
self, 'Touchsensor initialized sucessfully on port %s' %
self.__config['backTouchSensor'])
except Exception as exception:
self.logger.error(
self,
"Failed to initialize the Touch-Sensor - Are u sure it's connected to Port %s?"
% exception, exception)
self.logger.debug(self, "Sensor initialisation done")
def __initMotors(self):
'''Sub-method for initializing Motors.'''
self.logger.debug(self, "Starting motor initialisation...")
if self.__config['robotType'] == 'NORMAL':
try:
self.__lMotor = Motor(
self.__conf2port[self.__config['leftMotorPort']],
Direction.CLOCKWISE if
(not self.__config['leftMotorInverted']) else
Direction.COUNTERCLOCKWISE,
gears=self.__config['leftMotorGears'])
self.__rMotor = Motor(
self.__conf2port[self.__config['rightMotorPort']],
Direction.CLOCKWISE if
(not self.__config['rightMotorInverted']) else
Direction.COUNTERCLOCKWISE,
gears=self.__config['rightMotorGears'])
except Exception as exception:
self.logger.error(
self,
"Failed to initialize movement motors for robot type NORMAL - Are u sure they\'re all connected?",
exception)
if self.__config['useGearing']:
try:
self.__gearingPortMotor = Motor(
self.__conf2port[
self.__config['gearingSelectMotorPort']],
Direction.CLOCKWISE if
(not self.__config['gearingSelectMotorInverted']) else
Direction.COUNTERCLOCKWISE,
gears=self.__config['gearingSelectMotorGears'])
self.__gearingTurnMotor = Motor(
self.__conf2port[
self.__config['gearingTurnMotorPort']],
Direction.CLOCKWISE if
(not self.__config['gearingTurnMotorInverted']) else
Direction.COUNTERCLOCKWISE,
gears=self.__config['gearingTurnMotorGears'])
except Exception as exception:
self.logger.error(
self,
"Failed to initialize action motors for the gearing - Are u sure they\'re all connected?",
exception)
else:
try:
self.__aMotor1 = Motor(
self.__conf2port[
self.__config['firstActionMotorPort']],
Direction.CLOCKWISE if
(not self.__config['firstActionMotorInverted']) else
Direction.COUNTERCLOCKWISE,
gears=self.__config['firstActionMotorGears']) if (
self.__config['firstActionMotorPort'] != 0) else 0
self.__aMotor2 = Motor(
self.__conf2port[
self.__config['secondActionMotorPort']],
Direction.CLOCKWISE if
(not self.__config['secondActionMotorInverted']) else
Direction.COUNTERCLOCKWISE,
gears=self.__config['secondActionMotorGears']) if (
self.__config['secondActionMotorPort'] != 0) else 0
except Exception as exception:
self.logger.error(
self,
"Failed to initialize action motors - Are u sure they\'re all connected?",
exception)
else:
try:
self.__fRMotor = Motor(
self.__conf2port[self.__config['frontRightMotorPort']],
Direction.CLOCKWISE if
(not self.__config['frontRightMotorInverted']) else
Direction.COUNTERCLOCKWISE,
gears=self.__config['frontRightMotorGears']) if (
self.__config['frontRightMotorPort'] != 0) else 0
self.__bRMotor = Motor(
self.__conf2port[self.__config['backRightMotorPort']],
Direction.CLOCKWISE if
(not self.__config['backRightMotorInverted']) else
Direction.COUNTERCLOCKWISE,
gears=self.__config['backRightMotorGears']) if (
self.__config['backRightMotorPort'] != 0) else 0
self.__fLMotor = Motor(
self.__conf2port[self.__config['frontLeftMotorPort']],
Direction.CLOCKWISE if
(not self.__config['frontLeftMotorInverted']) else
Direction.COUNTERCLOCKWISE,
gears=self.__config['frontLeftMotorGears']) if (
self.__config['frontLeftMotorPort'] != 0) else 0
self.__bLMotor = Motor(
self.__conf2port[self.__config['backLeftMotorPort']],
Direction.CLOCKWISE if
(not self.__config['backLeftMotorInverted']) else
Direction.COUNTERCLOCKWISE,
gears=self.__config['backLeftMotorGears']) if (
self.__config['backLeftMotorPort'] != 0) else 0
except Exception as exception:
self.logger.error(
self,
"Failed to initialize movement motors for robot type %s - Are u sure they\'re all connected? Errored at Port"
% self.__config['robotType'], exception)
self.logger.debug(self, "Motor initialisation done")
self.logger.info(self, 'Charlie initialized')
def showDetails(self):
'''
Processes sensor data in a separate thread and shows
'''
threadLock = _thread.allocate_lock()
def __screenPrintRoutine():
while True:
if self.__gyro.angle() > 360:
ang = self.__gyro.angle() - 360
else:
ang = self.__gyro.angle()
speedRight = self.__rMotor.speed() if self.__config[
'robotType'] == 'NORMAL' else self.__fRMotor.speed()
speedRight = speedRight / 360 # from deg/s to revs/sec
speedRight = speedRight * (self.__config['wheelDiameter'] *
math.pi) # from revs/sec to cm/sec
speedLeft = self.__lMotor.speed() if self.__config[
'robotType'] == 'NORMAL' else self.__fLMotor.speed()
speedLeft = speedLeft / 360 # from deg/s to revs/sec
speedLeft = speedLeft * (self.__config['wheelDiameter'] *
math.pi) # from revs/sec to cm/sec
#self.brick.screen.set_font(Font(family = 'arial', size = 16))
if self.__screenRoutine:
print(self.__gyro.angle())
self.brick.screen.draw_text(5,
10,
'Robot-Angle: %s' % ang,
text_color=Color.BLACK,
background_color=Color.WHITE)
self.brick.screen.draw_text(5,
40,
'Right Motor Speed: %s' % ang,
text_color=Color.BLACK,
background_color=Color.WHITE)
self.brick.screen.draw_text(5,
70,
'Left Motor Speed: %s' % ang,
text_color=Color.BLACK,
background_color=Color.WHITE)
time.sleep(0.1)
with threadLock:
_thread.start_new_thread(__screenPrintRoutine, ())
def execute(self, params):
'''
This function interprets the number codes from the given array and executes the driving methods accordingly
Args:
params (array): The array of number code arrays to be executed
'''
if self.brick.battery.voltage() <= 7600:
if (self.__config["ignoreBatteryWarning"] == True):
self.logger.warn(
"Please charge the battery. Only %sV left. We recommend least 7.6 Volts for accurate and repeatable results. ignoreBatteryWarning IS SET TO True, THIS WILL BE IGNORED!!!"
% self.brick.battery.voltage() * 0.001)
else:
self.logger.warn(
"Please charge the battery. Only %sV left. We recommend least 7.6 Volts for accurate and repeatable results."
% self.brick.battery.voltage() * 0.001)
return
if self.__gyro == 0:
self.logger.error(self, "Cannot drive without gyro", '')
return
methods = {
3: self.absTurn,
4: self.turn,
5: self.action,
7: self.straight,
9: self.intervall,
11: self.curve,
12: self.toColor,
15: self.toWall
}
self.__gyro.reset_angle(0)
self.__gyro.reset_angle(0)
time.sleep(0.1)
self.__screenRoutine = True
while params != [] and not any(self.brick.buttons.pressed()):
pparams = params.pop(0)
mode, arg1, arg2, arg3 = pparams.pop(0), pparams.pop(
0), pparams.pop(0), pparams.pop(0)
methods[mode](arg1, arg2, arg3)
self.breakMotors()
if self.__config['useGearing']:
self.__gearingPortMotor.run_target(300, 0, Stop.HOLD,
True) # reset gearing
time.sleep(0.3)
self.__screenRoutine = False
def turn(self, speed, deg, port):
'''
Used to turn the motor on the spot using either one or both Motors for turning (2 or 4 in case of ALLWHEEL and MECANUM)
Args:
speed (int): the speed to drive at
deg (int): the angle to turn
port (int): the motor(s) to turn with
'''
startValue = self.__gyro.angle()
speed = self.min_speed if speed < self.min_speed else speed
# turn only with left motor
if port == 2:
# right motor off
self.__rMotor.dc(0)
# turn the angle
if deg > 0:
while self.__gyro.angle() - startValue < deg:
self.turnLeftMotor(speed)
# slow down to not overshoot
if not self.__gyro.angle() - startValue < deg * 0.6:
speed = speed - self._map(
deg, 1, 360, 10,
0.1) if speed > self.min_speed else self.min_speed
#cancel if button pressed
if any(self.brick.buttons.pressed()):
return
else:
while self.__gyro.angle() - startValue > deg:
self.turnLeftMotor(-speed)
# slow down to not overshoot
if not self.__gyro.angle() - startValue > deg * 0.6:
speed = speed - self._map(
deg, 1, 360, 10,
0.1) if speed > self.min_speed else self.min_speed
#cancel if button pressed
if any(self.brick.buttons.pressed()):
return
# turn only with right motor
elif port == 3:
# left motor off
self.__lMotor.dc(0)
# turn the angle
if deg > 0:
while self.__gyro.angle() - startValue < deg:
self.turnRightMotor(-speed)
# slow down to not overshoot
if not self.__gyro.angle() - startValue < deg * 0.6:
speed = speed - self._map(
deg, 1, 360, 10,
0.1) if speed > self.min_speed else self.min_speed
#cancel if button pressed
if any(self.brick.buttons.pressed()):
return
else:
while self.__gyro.angle() - startValue > deg:
self.turnRightMotor(speed)
# slow down to not overshoot
if not self.__gyro.angle() - startValue > deg * 0.6:
speed = speed - self._map(
deg, 1, 360, 10, 0.1
) if speed > self.min_speed else self.min_speed + 5
#cancel if button pressed
if any(self.brick.buttons.pressed()):
return
# turn with both motors
elif port == 23:
dualMotorbonus = 19
speed = speed * 2
# turn the angle
if deg > 0:
while self.__gyro.angle() - startValue < deg:
self.turnLeftMotor(speed / 2)
self.turnRightMotor(-speed / 2)
# slow down to not overshoot
if not self.__gyro.angle() - startValue < deg * 0.6:
speed = speed - self._map(
deg, 1, 360, 10, 0.01
) if speed - self._map(
deg, 1, 360, 10, 0.01
) > self.min_speed * 2 - dualMotorbonus else self.min_speed * 2 - dualMotorbonus
# cancel if button pressed
if any(self.brick.buttons.pressed()):
return
else:
while self.__gyro.angle() - startValue > deg:
self.turnLeftMotor(-speed / 2)
self.turnRightMotor(speed / 2)
# slow down to not overshoot
if not self.__gyro.angle() - startValue > deg * 0.6:
speed = speed - self._map(
deg, 1, 360, 10, 0.01
) if speed - self._map(
deg, 1, 360, 10, 0.01
) > self.min_speed * 2 - dualMotorbonus else self.min_speed * 2 - dualMotorbonus
# cancel if button pressed
if any(self.brick.buttons.pressed()):
return
def absTurn(self, speed, deg, port):
'''
Used to turn the motor on the spot using either one or both Motors for turning (2 or 4 in case of ALLWHEEL and MECANUM)
This method turns in contrast to the normal turn() method to an absolute ange (compared to starting point)
Args:
speed (int): the speed to drive at
deg (int): the angle to turn to
port (int): the motor(s) to turn with
'''
speed = self.min_speed if speed < self.min_speed else speed
# turn only with left motor
if port == 2:
# right motor off
self.__rMotor.dc(0)
# turn the angle
if deg > 0:
while self.__gyro.angle() < deg:
self.turnLeftMotor(speed)
# slow down to not overshoot
if not self.__gyro.angle() < deg * 0.6:
speed = speed - self._map(
deg, 1, 360, 10,
0.1) if speed > self.min_speed else self.min_speed
#cancel if button pressed
if any(self.brick.buttons.pressed()):
return
else:
while self.__gyro.angle() > deg:
self.turnLeftMotor(-speed)
# slow down to not overshoot
if not self.__gyro.angle() > deg * 0.6:
speed = speed - self._map(
deg, 1, 360, 10,
0.1) if speed > self.min_speed else self.min_speed
#cancel if button pressed
if any(self.brick.buttons.pressed()):
return
# turn only with right motor
elif port == 3:
# left motor off
self.__lMotor.dc(0)
# turn the angle
if deg > 0:
while self.__gyro.angle() < deg:
self.turnRightMotor(-speed)
# slow down to not overshoot
if not self.__gyro.angle() < deg * 0.6:
speed = speed - self._map(
deg, 1, 360, 10,
0.1) if speed > self.min_speed else self.min_speed
#cancel if button pressed
if any(self.brick.buttons.pressed()):
return
else:
while self.__gyro.angle() > deg:
self.turnRightMotor(speed)
# slow down to not overshoot
if not self.__gyro.angle() > deg * 0.6:
speed = speed - self._map(
deg, 1, 360, 10, 0.1
) if speed > self.min_speed else self.min_speed + 5
#cancel if button pressed
if any(self.brick.buttons.pressed()):
return
# turn with both motors
elif port == 23:
dualMotorbonus = 19
speed = speed * 2
# turn the angle
if deg > 0:
while self.__gyro.angle() < deg:
self.turnLeftMotor(speed / 2)
self.turnRightMotor(-speed / 2)
# slow down to not overshoot
if not self.__gyro.angle() < deg * 0.6:
speed = speed - self._map(
deg, 1, 360, 10, 0.01
) if speed - self._map(
deg, 1, 360, 10, 0.01
) > self.min_speed * 2 - dualMotorbonus else self.min_speed * 2 - dualMotorbonus
# cancel if button pressed
if any(self.brick.buttons.pressed()):
return
else:
while self.__gyro.angle() > deg:
self.turnLeftMotor(-speed / 2)
self.turnRightMotor(speed / 2)
# slow down to not overshoot
if not self.__gyro.angle() > deg * 0.6:
speed = speed - self._map(
deg, 1, 360, 10, 0.01
) if speed - self._map(
deg, 1, 360, 10, 0.01
) > self.min_speed * 2 - dualMotorbonus else self.min_speed * 2 - dualMotorbonus
# cancel if button pressed
if any(self.brick.buttons.pressed()):
return
def straight(self, speed, dist, ang):
'''
Drives the Robot in a straight line.
Also it self-corrects while driving with the help of a gyro-sensor. This is used to make the Robot more accurate
Args:
speed (int): the speed to drive at
dist (int): the distance in cm to drive
'''
if self.__config['robotType'] != 'MECANUM':
correctionStrength = 2.5 # how strongly the self will correct. 2 = default, 0 = nothing
startValue = self.__gyro.angle()
# convert the input (cm) to revs
revs = dist / (self.__config['wheelDiameter'] * math.pi)
motor = self.__rMotor if self.__config[
'robotType'] == 'NORMAL' else self.__fRMotor
# drive
motor.reset_angle(0)
if revs > 0:
while revs > (motor.angle() / 360):
# if not driving staright correct it
if self.__gyro.angle() - startValue > 0:
lSpeed = speed - abs(self.__gyro.angle() -
startValue) * correctionStrength
rSpeed = speed
elif self.__gyro.angle() - startValue < 0:
rSpeed = speed - abs(self.__gyro.angle() -
startValue) * correctionStrength
lSpeed = speed
else:
lSpeed = speed
rSpeed = speed
self.turnLeftMotor(lSpeed)
self.turnRightMotor(rSpeed)
#cancel if button pressed
if any(self.brick.buttons.pressed()):
return
else:
while revs < motor.angle() / 360:
# if not driving staright correct it
if self.__gyro.angle() - startValue < 0:
rSpeed = speed + abs(self.__gyro.angle() -
startValue) * correctionStrength
lSpeed = speed
elif self.__gyro.angle() - startValue > 0:
lSpeed = speed + abs(self.__gyro.angle() -
startValue) * correctionStrength
rSpeed = speed
else:
lSpeed = speed
rSpeed = speed
self.turnLeftMotor(-lSpeed)
self.turnRightMotor(-rSpeed)
# cancel if button pressed
if any(self.brick.buttons.pressed()):
return
else:
self.__fRMotor.reset_angle(0)
# convert the input (cm) to revs
revs = dist / (self.__config['wheelDiameter'] * math.pi)
speed = speed * 1.7 * 6 # convert speed form % to deg/min
# driving the robot into the desired direction
if ang >= 0 and ang <= 45:
multiplier = _map(ang, 0, 45, 1, 0)
self.__fRMotor.run_angle(speed, revs * 360, Stop.COAST, False)
self.__bRMotor.run_angle(speed * multiplier + 1,
revs * 360 * multiplier, Stop.COAST,
False)
self.__fLMotor.run_angle(speed * multiplier + 1,
revs * 360 * multiplier, Stop.COAST,
False)
self.__bLMotor.run_angle(speed, revs * 360, Stop.COAST, True)
elif ang >= -45 and ang < 0:
multiplier = _map(ang, -45, 0, 0, 1)
self.__fRMotor.run_angle(speed * multiplier + 1,
revs * 360 * multiplier, Stop.COAST,
False)
self.__bRMotor.run_angle(speed, revs * 360, Stop.COAST, False)
self.__bLMotor.run_angle(speed * multiplier + 1,
revs * 360 * multiplier, Stop.COAST,
False)
self.__fLMotor.run_angle(speed, revs * 360, Stop.COAST, True)
elif ang > 45 and ang <= 90:
multiplier = _map(ang, 45, 90, 0, 1)
self.__fRMotor.run_angle(speed, revs * 360, Stop.COAST, False)
self.__bRMotor.run_angle(speed * multiplier + 1,
revs * -360 * multiplier, Stop.COAST,
False)
self.__fLMotor.run_angle(speed * multiplier + 1,
revs * -360 * multiplier, Stop.COAST,
False)
self.__bLMotor.run_angle(speed, revs * 360, Stop.COAST, True)
elif ang < -45 and ang >= -90:
multiplier = _map(ang, -45, -90, 0, 1)
self.__fRMotor.run_angle(speed * multiplier + 1,
revs * -360 * multiplier, Stop.COAST,
False)
self.__bRMotor.run_angle(speed, revs * 360, Stop.COAST, False)
self.__bLMotor.run_angle(speed * multiplier + 1,
revs * -360 * multiplier, Stop.COAST,
False)
self.__fLMotor.run_angle(speed, revs * 360, Stop.COAST, True)
elif ang > 90 and ang <= 135:
multiplier = _map(ang, 90, 135, 1, 0)
self.__fRMotor.run_angle(speed * multiplier + 1,
revs * 360 * multiplier, Stop.COAST,
False)
self.__bRMotor.run_angle(speed, revs * -360, Stop.COAST, False)
self.__bLMotor.run_angle(speed * multiplier + 1,
revs * 360 * multiplier, Stop.COAST,
False)
self.__fLMotor.run_angle(speed, revs * -360, Stop.COAST, True)
elif ang < -90 and ang >= -135:
multiplier = _map(ang, -90, -135, 1, 0)
self.__fRMotor.run_angle(speed, revs * -360, Stop.COAST, False)
self.__bRMotor.run_angle(speed * multiplier + 1,
revs * 360 * multiplier, Stop.COAST,
False)
self.__fLMotor.run_angle(speed * multiplier + 1,
revs * 360 * multiplier, Stop.COAST,
False)
self.__bLMotor.run_angle(speed, revs * -360, Stop.COAST, True)
elif ang > 135 and ang <= 180:
multiplier = _map(ang, 135, 180, 0, 1)
self.__fRMotor.run_angle(speed * multiplier + 1,
revs * -360 * multiplier, Stop.COAST,
False)
self.__bRMotor.run_angle(speed, revs * -360, Stop.COAST, False)
self.__bLMotor.run_angle(speed * multiplier + 1,
revs * -360 * multiplier, Stop.COAST,
False)
self.__fLMotor.run_angle(speed, revs * -360, Stop.COAST, True)
elif ang < -135 and ang >= -180:
multiplier = _map(ang, -135, -180, 0, 1)
self.__fRMotor.run_angle(speed, revs * -360, Stop.COAST, False)
self.__bRMotor.run_angle(speed * multiplier + 1,
revs * -360 * multiplier, Stop.COAST,
False)
self.__fLMotor.run_angle(speed * multiplier + 1,
revs * -360 * multiplier, Stop.COAST,
False)
self.__bLMotor.run_angle(speed, revs * -360, Stop.COAST, True)
def intervall(self, speed, dist, count):
'''
Drives forwads and backwards x times.
Args:
speed (int): the speed to drive at
revs (int): the distance (in cm) to drive
count (int): how many times it should repeat the driving
'''
# convert the input (cm) to revs
revs = dist / (self.__config['wheelDiameter'] * math.pi) / 2
speed = speed * 1.7 * 6 # speed in deg/s to %
# move count times forwards and backwards
for i in range(count + 1):
if self.__config['robotType'] == 'NORMAL':
ang = self.__lMotor.angle()
# drive backwards
self.__rMotor.run_angle(speed, revs * -360, Stop.BRAKE, False)
self.__lMotor.run_angle(speed, revs * -360, Stop.BRAKE, False)
# return to cancel if any button is pressed
while self.__lMotor.angle() > revs * -360:
if any(self.brick.buttons.pressed()):
return
# drive forwards
self.__lMotor.run_angle(speed, revs * 360, Stop.BRAKE, False)
self.__rMotor.run_angle(speed, revs * 360, Stop.BRAKE, False)
# return to cancel if any button is pressed
while self.__rMotor.angle() <= ang:
if any(self.brick.buttons.pressed()):
return
elif self.__config['robotType'] == 'ALLWHEEL' or self.__config[
'robotType'] == 'MECANUM':
ang = self.__lMotor.angle()
# drive backwards
self.__fRMotor.run_angle(speed, revs * -360, Stop.BRAKE, False)
self.__bRMotor.run_angle(speed, revs * -360, Stop.BRAKE, False)
self.__fLMotor.run_angle(speed, revs * -360, Stop.BRAKE, False)
self.__bLMotor.run_angle(speed, revs * -360, Stop.BRAKE, False)
# return to cancel if any button is pressed
while self.__lMotor.angle() > revs * -360:
if any(self.brick.buttons.pressed()):
return
# drive forwards
self.__fRMotor.run_angle(speed, revs * 360, Stop.BRAKE, False)
self.__bRMotor.run_angle(speed, revs * 360, Stop.BRAKE, False)
self.__fLMotor.run_angle(speed, revs * 360, Stop.BRAKE, False)
self.__bLMotor.run_angle(speed, revs * 360, Stop.BRAKE, False)
# return to cancel if any button is pressed
while self.__rMotor.angle() <= ang:
if any(self.brick.buttons.pressed()):
return
def curve(self, speed, dist, deg):
'''
Drives forwads and backwards x times.
Args:
speed (int): the speed to drive at
revs1 (int): the distance (in motor revolutions) for the outer wheel to drive
deg (int): how much of a circle it should drive
'''
speed = speed * 1.7 * 6 # speed to deg/s from %
# gyro starting point
startValue = self.__gyro.angle()
revs1 = dist / (self.__config['wheelDiameter'] * math.pi)
# claculate revs for the second wheel
pathOutside = self.__config['wheelDiameter'] * math.pi * revs1
rad1 = pathOutside / (math.pi * (deg / 180))
rad2 = rad1 - self.__config['wheelDistance']
pathInside = rad2 * math.pi * (deg / 180)
revs2 = pathInside / (self.__config['wheelDiameter'] * math.pi)
# claculate the speed for the second wheel
relation = revs1 / revs2
speedSlow = speed / relation
if deg > 0:
# asign higher speed to outer wheel
lSpeed = speed
rSpeed = speedSlow
self.__rMotor.run_angle(rSpeed, revs2 * 360, Stop.COAST, False)
self.__lMotor.run_angle(lSpeed, revs1 * 360 + 5, Stop.COAST, False)
#turn
while self.__gyro.angle() - startValue < deg and not any(
self.brick.buttons.pressed()):
pass
else:
# asign higher speed to outer wheel
lSpeed = speed
rSpeed = speedSlow
self.__rMotor.run_angle(rSpeed, revs2 * 360 + 15, Stop.COAST,
False)
self.__lMotor.run_angle(lSpeed, revs1 * 360, Stop.COAST, False)
#turn
while self.__gyro.angle() - startValue > deg and not any(
self.brick.buttons.pressed()):
pass
def toColor(self, speed, color, side):
'''
Drives forward until the given colorSensor sees a given color.
Args:
speed (int): the speed to drive at
color (int): the color to look for (0 = Black, 1 = White)
side (int): which side's color sensor should be used
'''
# sets color to a value that the colorSensor can work with
if color == 0:
color = Color.BLACK
else:
color = Color.WHITE
# Refactor code
# only drive till left colorSensor
if side == 2:
# if drive to color black drive until back after white to not recognize colors on the field as lines
if color == Color.BLACK:
while lLight.color() != Color.WHITE and not any(
self.brick.buttons.pressed()):
self.turnBothMotors(speed)
while lLight.color() != color and not any(
self.brick.buttons.pressed()):
self.turnBothMotors(speed)
# only drive till right colorSensor
elif side == 3:
# if drive to color black drive until back after white to not recognize colors on the field as lines
if color == Color.BLACK:
while rLight.color() != Color.WHITE and not any(
self.brick.buttons.pressed()):
self.turnBothMotors(speed)
while rLight.color() != color and not any(
self.brick.buttons.pressed()):
self.turnBothMotors(speed)
# drive untill both colorSensors
elif side == 23:
rSpeed = speed
lSpeed = speed
rWhite = False
lWhite = False
while (rLight.color() != color or lLight.color() != color
) and not any(self.brick.buttons.pressed()):
#if drive to color black drive until back after white to not recognize colors on the field as lines
if color == Color.BLACK:
if rLight.color() == Color.WHITE:
rWhite = True
if lLight.color() == Color.WHITE:
lWhite = True
self.__rMotor.dc(rSpeed)
self.__lMotor.dc(lSpeed)
# if right at color stop right Motor
if rLight.color() == color and rWhite:
rSpeed = 0
# if left at color stop left Motor
if lLight.color() == color and lWhite:
lSpeed = 0
def toWall(self, speed, *args):
'''
Drives until a pressure sensor is pressed
Args:
speed (int): the speed to drive at
'''
while not touch.pressed():
self.turnBothMotors(-abs(speed))
if any(self.brick.buttons()):
break
self.turnBothMotors(0)
def action(self, speed, revs, port):
'''
Doesn't drive the robot, but drives the action motors
Args:
speed (int): the speed to turn the motor at
revs (int): how long to turn the motor for
port (int): which one of the motors should be used
'''
speed = abs(speed) * 1.7 * 6 # speed to deg/s from %
if self.__config['useGearing']:
self.__gearingPortMotor.run_target(300, port * 90, Stop.HOLD,
True) # select gearing Port
ang = self.__gearingTurnMotor.angle()
self.__gearingTurnMotor.run_angle(speed, revs * 360, Stop.BRAKE,
False) # start turning the port
# cancel, if any brick button is pressed
if revs > 0:
while self.__gearingTurnMotor.angle() < revs * 360 - ang:
if any(self.brick.buttons.pressed()):
self.__gearingTurnMotor.dc(0)
return
else:
while self.__gearingTurnMotor.angle() > revs * 360 + ang:
if any(self.brick.buttons.pressed()):
self.__gearingTurnMotor.dc(0)
return
else:
# turn motor 1
if port == 1:
ang = self.__aMotor1.angle()
self.__aMotor1.run_angle(speed, revs * 360, Stop.HOLD, False)
if revs > 0:
while self.__aMotor1.angle() < revs * 360 - ang:
if any(self.brick.buttons.pressed()):
self.__aMotor1.dc(0)
return
else:
while self.__aMotor1.angle() > revs * 360 + ang:
if any(self.brick.buttons.pressed()):
self.__aMotor1.dc(0)
return
# turm motor 2
elif port == 2:
ang = self.__aMotor2.angle()
self.__aMotor2.run_angle(speed, revs * 360, Stop.HOLD, False)
if revs > 0:
while self.__aMotor2.angle() < revs * 360 - ang:
if any(self.brick.buttons.pressed()):
self.__aMotor2.dc(0)
return
else:
while self.__aMotor2.angle() > revs * 360 + ang:
if any(self.brick.buttons.pressed()):
self.__aMotor2.dc(0)
return
def turnLeftMotor(self, speed):
'''
Sub-method for driving the left Motor(s)
Args:
speed (int): the speed to drive the motor at
'''
if self.__config['robotType'] == 'NORMAL':
self.__lMotor.dc(speed)
else:
self.__fLMotor.dc(speed)
self.__bLMotor.dc(speed)
def turnRightMotor(self, speed):
'''
Sub-method for driving the right Motor(s)
Args:
speed (int): the speed to drive the motor at
'''
if self.__config['robotType'] == 'NORMAL':
self.__rMotor.dc(speed)
else:
self.__fRMotor.dc(speed)
self.__bRMotor.dc(speed)
def turnBothMotors(self, speed):
'''
Submethod for setting a motor.dc() to all motors
Args:
speed (int): the speed (in percent) to set the motors to
'''
if self.__config['robotType'] == 'NORMAL':
self.__rMotor.dc(speed)
self.__lMotor.dc(speed)
else:
self.__fRMotor.dc(speed)
self.__bRMotor.dc(speed)
self.__fLMotor.dc(speed)
self.__bLMotor.dc(speed)
def breakMotors(self):
'''Sub-method for breaking all the motors'''
if self.__config['robotType'] == 'NORMAL':
self.__lMotor.hold()
self.__rMotor.hold()
else:
self.__fRMotor.hold()
self.__bRMotor.hold()
self.__fLMotor.hold()
self.__bLMotor.hold()
time.sleep(0.2)
def _map(self, x, in_min, in_max, out_min, out_max):
'''
Converts a given number in the range of two numbers to a number in the range of two other numbers
Args:
x (int): the input number that should be converted
in_min (int): The minimal point of the range of input number
in_max (int): The maximal point of the range of input number
out_min (int): The minimal point of the range of output number
out_max (int): The maximal point of the range of output number
Returns:
int: a number between out_min and out_max, de
'''
return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min
def getGyroAngle(self):
return self.__gyro.angle()
def setRemoteValues(self, data):
x = data['x']
y = data['y']
if x == 0:
x = 0.0001
if data['y'] == 0 and data['x'] == 0:
self.breakMotors()
else:
radius = int(math.sqrt(x**2 + y**2)) # distance from center
ang = math.atan(y / x) # angle in radians
a = int(self._map(radius, 0, 180, 0, 100))
b = int(-1 * math.cos(2 * ang) * self._map(radius, 0, 180, 0, 100))
if x < 0:
temp = a
a = b
b = temp
if y < 0:
temp = a
a = -b
b = -temp
self.turnLeftMotor(int(self._map(a, 0, 100, 0, data['maxSpeed'])))
self.turnRightMotor(int(self._map(b, 0, 100, 0, data['maxSpeed'])))
if data['a1'] == 0:
self.__aMotor1.hold()
else:
a1Speed = data['a1']
self.__aMotor1.dc(a1Speed)
#!/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
from pybricks.tools import wait, StopWatch, DataLog
from pybricks.robotics import DriveBase
from pybricks.media.ev3dev import SoundFile, ImageFile
# 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()
motor = Motor(Port.B)
# Write your program here.
motor.run_target(10, 90)
if gyro_sensor.angle() < -3:
break
lift_motor.stop(Stop.HOLD)
# Move the robot forward for some time using the front and rear
# motors.
front_motor.dc(60)
rear_motor.dc(100)
wait(1300)
# Play a sound and pull the rear structure up so it gets back to
# its starting position. Keep moving forward slowly by
# simultaneously running the front and rear motors.
brick.sound.file(SoundFile.AIR_RELEASE)
front_motor.dc(30)
rear_motor.dc(30)
lift_motor.run_target(160, 0, Stop.HOLD)
# Update the "steps" variable and display it on the screen.
steps -= 1
brick.display.clear()
brick.display.text(steps, (90, 70))
# Settle the robot at the top of a step and end the program.
front_motor.dc(100)
rear_motor.dc(90)
wait(2000)
front_motor.stop(Stop.HOLD)
rear_motor.stop(Stop.HOLD)
wait(5000)
green = "yes"
while green == "yes":
x = cl.color()
print(x)
while x == Color.RED:
x = cl.color()
robot.drive(50, 0)
x = cl.color()
print(x)
if grabbed == 0:
if distance_sensor.distance(
) < 50 and distance_sensor.distance() > 40:
back = "yes"
robot.stop()
time.sleep(2)
arm_motor.run_target(400, -65)
time.sleep(2)
#x="none"
grabbed += 1
robot.straight(-815)
right()
robot.straight(-350)
ev3.speaker.play_file(SoundFile.TOUCH)
time.sleep(3)
ts = TouchSensor(Port.S1).pressed()
if ts == True:
arm_motor.run_target(400, 60)
green = "no"
BREAK = "yes"
break
#!/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
Tarttuja = Motor(Port.A)
Tarttuja.run_until_stalled(200, Stop.COAST, 50)
Tarttuja.reset_angle(0)
Tarttuja.run_target(200, -90)
def robot_pick(position):
# Close the gripper to grab the wheel stack.
Tarttuja.run_until_stalled(200, Stop.HOLD, 50)
def robot_release(position):
# Open the gripper to release the wheel stack.
Tarttuja.run_target(200, -90)
LEFT = 160
MIDDLE = 100
RIGHT = 40
while Button.LEFT not in brick.buttons():
wait(1)
white = color_sensor.reflection()
brick.sound.beep()
while Button.RIGHT not in brick.buttons():
wait(1)
black = color_sensor.reflection()
brick.sound.beep()
deciding_value = (white + black)/2
print("Deciding value = %s, white = %s. black = %s" % (deciding_value, white, black))
while True:
if color_sensor.reflection() < deciding_value: #Checks to see if the robot is on line
brick.display.text("%s Sensed line" % color_sensor.reflection())
print("%s Sensed line" % color_sensor.reflection())
moving_motor.reset_angle(0)
print("reset angle")
moving_motor.run_target(500, -90) #Moves robot forward
print("moved forward")
else:
brick.display.text("%s Nothing sensed, searching." % color_sensor.reflection())
incrament = 2
a = incrament
b = 1
while color_sensor.reflection() > deciding_value:
rotating_motor.run_target(500,a*b)
a = a + incrament
b = - b
print("searching")
print("end of else")
print("end of while")
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)
# Initialize the base. First rotate it until the Touch Sensor
# in the base is pressed. Reset the motor angle to make this
# the zero point. Then hold the motor in place so it does not move.
base_motor.run(-60)
while not base_switch.pressed():
wait(10)
base_motor.reset_angle(0)
base_motor.hold()
# Initialize the gripper. First rotate the motor until it stalls.
# Stalling means that it cannot move any further. This position
# corresponds to the closed position. Then rotate the motor
# by 90 degrees such that the gripper is open.
gripper_motor.run_until_stalled(200, then=Stop.COAST, duty_limit=50)
gripper_motor.reset_angle(0)
gripper_motor.run_target(200, -90)
def robot_pick(position):
# This function makes the robot base rotate to the indicated
# position. There it lowers the elbow, closes the gripper, and
# raises the elbow to pick up the object.
# Rotate to the pick-up position.
base_motor.run_target(60, position)
# Lower the arm.
elbow_motor.run_target(60, -40)
# Close the gripper to grab the wheel stack.
gripper_motor.run_until_stalled(200, then=Stop.HOLD, duty_limit=50)
# Raise the arm to lift the wheel stack.
elbow_motor.run_target(60, 0)
while True:
if l_color.ambient()>5:
r_motor.run_time(0,0)
l_motor.run_time(0,0)
ev3.screen.print(l_color.ambient())
wait(500)
ev3.screen.print()
break
else:
l_motor.run_time(-50,100)
ev3.screen.print(l_motor.angle())
wait(50)
n = l_motor.angle()
l_motor.run_target(50, -n/2)
ev3.screen.print(l_color.ambient())
wait(50)
#r_motor
while True:
if r_color.ambient()>5:
r_motor.run_time(0,0)
l_motor.run_time(0,0)
ev3.screen.print(r_color.ambient())
wait(50)
ev3.screen.print()
break
else:
r_motor.run_time(50,100)
ev3.screen.print(r_color.ambient())
class Rac3Truck:
WHEEL_DIAMETER = 30 # milimeters
AXLE_TRACK = 120 # milimeters
def __init__(
self,
left_motor_port: str = Port.B, right_motor_port: str = Port.C,
polarity: str = 'inversed',
steer_motor_port: str = Port.A,
ir_sensor_port: str = Port.S4, ir_beacon_channel: int = 1):
if polarity == 'normal':
self.left_motor = Motor(port=left_motor_port,
positive_direction=Direction.CLOCKWISE)
self.right_motor = Motor(port=right_motor_port,
positive_direction=Direction.CLOCKWISE)
else:
self.left_motor = \
Motor(port=left_motor_port,
positive_direction=Direction.COUNTERCLOCKWISE)
self.right_motor = \
Motor(port=right_motor_port,
positive_direction=Direction.COUNTERCLOCKWISE)
self.driver = DriveBase(left_motor=self.left_motor,
right_motor=self.right_motor,
wheel_diameter=self.WHEEL_DIAMETER,
axle_track=self.AXLE_TRACK)
self.steer_motor = Motor(port=steer_motor_port,
positive_direction=Direction.CLOCKWISE)
self.ir_sensor = InfraredSensor(port=ir_sensor_port)
self.ir_beacon_channel = ir_beacon_channel
def reset(self):
self.steer_motor.run_time(
speed=300,
time=1500,
then=Stop.COAST,
wait=True)
self.steer_motor.run_angle(
speed=-500,
rotation_angle=120,
then=Stop.HOLD,
wait=True)
self.steer_motor.reset_angle(angle=0)
def steer_left(self):
if self.steer_motor.angle() > -65:
self.steer_motor.run_target(
speed=-200,
target_angle=-65,
then=Stop.HOLD,
wait=True)
else:
self.steer_motor.hold()
def steer_right(self):
if self.steer_motor.angle() < 65:
self.steer_motor.run_target(
speed=200,
target_angle=65,
then=Stop.HOLD,
wait=True)
else:
self.steer_motor.hold()
def steer_center(self):
if self.steer_motor.angle() < -7:
self.steer_motor.run_target(
speed=200,
target_angle=4,
then=Stop.HOLD,
wait=True)
elif self.steer_motor.angle() > 7:
self.steer_motor.run_target(
speed=-200,
target_angle=-4,
then=Stop.HOLD,
wait=True)
self.steer_motor.hold()
wait(100)
def drive_by_ir_beacon(self):
ir_beacon_button_pressed = \
set(self.ir_sensor.buttons(channel=self.ir_beacon_channel))
# forward
if ir_beacon_button_pressed == {Button.LEFT_UP, Button.RIGHT_UP}:
self.driver.drive(
speed=800,
turn_rate=0)
self.steer_center()
# backward
elif ir_beacon_button_pressed == {Button.LEFT_DOWN, Button.RIGHT_DOWN}:
self.driver.drive(
speed=-800,
turn_rate=0)
self.steer_center()
# turn left forward
elif ir_beacon_button_pressed == {Button.LEFT_UP}:
self.left_motor.run(speed=600)
self.right_motor.run(speed=1000)
self.steer_left()
# turn right forward
elif ir_beacon_button_pressed == {Button.RIGHT_UP}:
self.left_motor.run(speed=1000)
self.right_motor.run(speed=600)
self.steer_right()
# turn left backward
elif ir_beacon_button_pressed == {Button.LEFT_DOWN}:
self.left_motor.run(speed=-600)
self.right_motor.run(speed=-1000)
self.steer_left()
# turn right backward
elif ir_beacon_button_pressed == {Button.RIGHT_DOWN}:
self.left_motor.run(speed=-1000)
self.right_motor.run(speed=-600)
self.steer_right()
# otherwise stop
else:
self.driver.stop()
self.steer_center()
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
def BlackandGreen():
#!/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
from pybricks.tools import wait, StopWatch, DataLog
from pybricks.robotics import DriveBase
from pybricks.media.ev3dev import SoundFile, ImageFile
# This program requires LEGO EV3 MicroPython v2.0 or higher.
# Click "Open user guide" on the EV3 extension tab for more information.
#define your variables
ev3 = EV3Brick()
left_motor = Motor(Port.C)
right_motor = Motor(Port.B)
medium_motor = Motor(Port.A)
large_motor = Motor(Port.D)
wheel_diameter = 56
axle_track = 115
line_sensor = ColorSensor(Port.S2)
line_sensor1 = ColorSensor(Port.S3)
robot = DriveBase(left_motor, right_motor, wheel_diameter, axle_track)
#follows the line underneath the pull up bar until the leftsensor detects black
BLACK = 9
WHITE = 85
threshold = (BLACK + WHITE) / 2
# Set the drive speed at 100 millimeters per second.
DRIVE_SPEED = 100
# Set the gain of the proportional line controller. This means that for every
PROPORTIONAL_GAIN = 1.2
runWhile = True
#goes straight to get ready for line following then resets the distance
robot.straight(250)
robot.reset()
#starts to follow the line towards the replay logo
while True:
# Calculate the deviation from the threshold.
deviation = line_sensor.reflection() - threshold
# Calculate the turn rate.
turn_rate = PROPORTIONAL_GAIN * deviation
# Set the drive base speed and turn rate.
robot.drive(DRIVE_SPEED, turn_rate)
wait(10)
print(robot.distance())
if (robot.distance() >= 450):
robot.stop(Stop.BRAKE)
break
#the robot pushes the phone into the replay logo and moves back to get ready to drop the health units into the replay logo
robot.straight(-75)
robot.stop(Stop.BRAKE)
#the robot then turns so it is going to be perfectly into the replay logo
robot.turn(-35)
#the robot drops the health units
large_motor.run_angle(100, 150)
#then turns to an angle to go back to base
robot.turn(50)
robot.straight(-1000)
wait(50)
#!/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
from pybricks.tools import wait, StopWatch, DataLog
from pybricks.robotics import DriveBase
from pybricks.media.ev3dev import SoundFile, ImageFile
# 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()
left_motor = Motor(Port.C)
right_motor = Motor(Port.B)
medium_motor = Motor(Port.A)
front_largeMotor = Motor(Port.D)
wheel_diameter = 56
axle_track = 114.3
robot = DriveBase(left_motor, right_motor, wheel_diameter, axle_track)
## Write your code here:
## The robot goes straight until the Boccia Mission's target.
robot.straight(1060)
## The robot moves the large motor down to drop the cubes in the target.
front_largeMotor.run_angle(80, 70, then=Stop.HOLD, wait=True)
front_largeMotor.run_angle(-80, 70, then=Stop.HOLD, wait=True)
## Dance Mission
## The robot moves backwards to reach the Dance Floor so it can Dance as the last mission.
robot.straight(-185)
robot.turn(-70)
robot.straight(138)
## The following code is all the dance moves we do for the Dance Mission.
robot.turn(160)
robot.turn(-160)
robot.straight(60)
front_largeMotor.run_target(500, 60)
front_largeMotor.run_target(500, -40)
robot.straight(-60)
robot.turn(260)
robot.turn(-260)
robot.turn(100)
robot.straight(40)
robot.turn(100)
front_largeMotor.run_angle(500, 30)
#!/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
motor_a = Motor(Port.A)
brick.sound.beep()
wait(1000)
motor_a.run_target(500, 720) #500 degrees per second, 90 target angle
wait(1000)
brick.sound.beep(1000, 500) #frequency, duration
wait(2000)
# Play a sound and show an image to indicate that we are done scanning.
ev3.speaker.play_file(SoundFile.READY)
ev3.screen.load_image(ImageFile.EV3)
# Now sort the bricks according the list of colors that we stored.
# We do this by going over each color in the list in a loop.
for color in color_list:
# Wait for one second between each sorting action.
wait(1000)
# Run the conveyor belt motor to the right position based on the color.
if color == Color.BLUE:
ev3.speaker.say('blue')
belt_motor.run_target(500, 10)
elif color == Color.GREEN:
ev3.speaker.say('green')
belt_motor.run_target(500, 132)
elif color == Color.YELLOW:
ev3.speaker.say('yellow')
belt_motor.run_target(500, 360)
elif color == Color.RED:
ev3.speaker.say('red')
belt_motor.run_target(500, 530)
# Now that the conveyor belt is in the correct position, eject the
# colored object.
feed_motor.run_angle(1500, 180)
feed_motor.run_angle(1500, -180)
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()
