def move_to_white( power, port, brake ):
move_forever( power )
color_sensor=ColorSensor( port )
print(color_sensor.reflection())
while color_sensor.reflection()<90:
print(color_sensor.reflection())
wait(0.1)
stop( brake )
brick.sound.beep()
class Sweep3r(IRBeaconRemoteControlledTank, EV3Brick):
WHEEL_DIAMETER = 40
AXLE_TRACK = 110
def __init__(
self,
left_foot_motor_port: Port = Port.B, right_foot_motor_port: Port = Port.C,
medium_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):
super().__init__(
wheel_diameter=self.WHEEL_DIAMETER, axle_track=self.AXLE_TRACK,
left_motor_port=left_foot_motor_port, right_motor_port=right_foot_motor_port,
ir_sensor_port=ir_sensor_port, ir_beacon_channel=ir_beacon_channel)
self.medium_motor = Motor(port=medium_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 drill(self):
while True:
if Button.BEACON in self.ir_sensor.buttons(channel=self.ir_beacon_channel):
self.medium_motor.run_angle(
speed=1000,
rotation_angle=2 * 360,
then=Stop.HOLD,
wait=True)
def move_when_touched(self):
while True:
if self.touch_sensor.pressed():
self.drive_base.turn(angle=100)
def move_when_see_smothing(self):
while True:
if self.color_sensor.reflection() > 30:
self.drive_base.turn(angle=-100)
def main(self, speed: float = 1000):
self.screen.load_image(ImageFile.PINCHED_MIDDLE)
Process(target=self.move_when_touched).start()
Process(target=self.move_when_see_smothing).start()
Process(target=self.drill).start()
self.keep_driving_by_ir_beacon(speed=speed)
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 CuriosityRov3r(IRBeaconRemoteControlledTank, EV3Brick):
WHEEL_DIAMETER = 35 # milimeters
AXLE_TRACK = 130 # milimeters
def __init__(
self,
left_motor_port: Port = Port.B, right_motor_port: Port = Port.C,
medium_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):
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.medium_motor = Motor(port=medium_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 spin_fan(self, speed: float = 1000):
while True:
if self.color_sensor.reflection() > 20:
self.medium_motor.run(speed=speed)
else:
self.medium_motor.stop()
def say_when_touched(self):
while True:
if self.touch_sensor.pressed():
self.screen.load_image(ImageFile.ANGRY)
self.speaker.play_file(file=SoundFile.NO)
self.medium_motor.run_time(
speed=-500,
time=3000,
then=Stop.HOLD,
wait=True)
def main(self, speed: float = 1000):
run_parallel(
self.say_when_touched,
self.spin_fan,
self.keep_driving_by_ir_beacon)
def move_to_black_for_square( power, port1, port2, brake ):
move_forever( power )
light_sensor1=ColorSensor( port1 )
light_sensor2=ColorSensor( port2 )
print(str(light_sensor1.reflection())+ " " + str(light_sensor2.reflection()))
while light_sensor1.reflection() > 15 and light_sensor2.reflection() > 15:
print(str(light_sensor1.reflection())+ " " + str(light_sensor2.reflection()))
wait(0.1)
print(str(light_sensor1.reflection())+ " " + str(light_sensor2.reflection()))
stop( brake )
brick.sound.beep()
def run_on_color():
color_sensor = ColorSensor(Port.S1)
color = color_sensor.color()
print(color)
data.log('Color detected', color)
rgb_value = color_sensor.rgb()
print('Color detected RGB', rgb_value)
data.log('Color detected RGB', rgb_value)
ambient = color_sensor.ambient()
print('Color detected ambient', ambient)
data.log('Color detected ambient', ambient)
reflection = color_sensor.reflection()
print('Color detected reflection', reflection)
data.log('Color detected reflection', reflection)
#reset gyro before start of program
gyro_sensor.reset_angle(0)
if (color == Color.WHITE):
ev3.speaker.say('white')
print("Jolene Run")
jolene_run()
#jolene_test()
elif (color == Color.BLUE):
#ev3.speaker.say('blue')
print("Jason Run")
jason_start_slide()
#
#jolene_test()
elif (color == Color.RED):
#ev3.speaker.say('red')
print("Sophie Run")
sophie_run()
elif (color == Color.BLACK):
jason_step_counter()
elif (color == Color.GREEN):
sophie_bench()
else:
rgb_value = color_sensor.rgb()
print(rgb_value)
#gyro_test()
jolene_run()
class LineFollower:
def __init__(self, controller):
self.leftMotor = Motor(Port.B,
positive_direction=Direction.COUNTERCLOCKWISE)
self.rightMotor = Motor(Port.C,
positive_direction=Direction.COUNTERCLOCKWISE)
self.lightSensor = ColorSensor(Port.S3)
self.controller = controller
def run(self):
while True:
light = self.lightSensor.reflection()
left, right = self.controller.getPower(float(light))
message = 'Z:%d L:%d R:%d' % (light, left, right)
ev3.screen.print(message)
self.leftMotor.run(left)
self.rightMotor.run(right)
def displayLightValue(port):
"Continously prints all values from color sensor at given port"
# creat the sensor object from the ColorSensor class
sensor = ColorSensor(port)
while True:
# have four different ways of using this
# sensor!
color = sensor.color()
reflection = sensor.reflection()
ambient = sensor.ambient()
rgb = sensor.rgb()
print("color: ", color)
print("reflection: ", reflection)
print("ambient: ", ambient)
print("rgb: ", rgb)
wait(1000)
def line_follower(distance, speed, gain=1.2, right_side=True, delay=10):
"""
Based on https://pybricks.github.io/ev3-micropython/examples/robot_educator_line.html
:param gain: the gain of the proportional line controller. This means that for every
percentage point of light deviating from the threshold, we set the turn
rate of the drivebase to 1.2 degrees per second.
"""
# This will measure the distance driven by using the "distance()" and "reset()" functions,
# which are kind of like a Trip Odometer in a regular car.
# Calculate the light threshold. Choose values based on your measurements.
BLACK = 9
WHITE = 85
threshold = (BLACK + WHITE) / 2
# Initialize the color sensor.
line_sensor = ColorSensor(Port.S2)
flip = -1 if right_side else 1
robot.reset()
gone_distance = 0
# this loop goes until we've gone enough distance, then the condition will be false
while gone_distance <= distance:
# Calculate the deviation from the threshold.
deviation = line_sensor.reflection() - threshold
# Calculate the turn rate.
turn_rate = gain * deviation
# Set the drive base speed and turn rate.
robot.drive(speed, flip * turn_rate)
# You can wait for a short time or do other things in this loop.
wait(delay)
# TODO: figure out how far we just went
gone_distance = robot.distance()
print(gone_distance)
class colorSensor:
""" Defines a color sensor component. """
def __init__(self, sensor_input):
"""
Initializes the sensor color component.
:param sensor_input: the input pin of the sensor
"""
self._sensor = ColorSensor(sensor_input)
def read_intensity(self):
"""
Reads the reflected light intensity from the color sensor.
:return: the intensity in [0; 100]
"""
return self._sensor.reflection()
def read_color(self):
"""
Reads the color from the color sensor.
:return: the index of the found color
"""
return self._sensor.rgb()
def LineFollow(distance):
# Initialize the color sensor.
line_sensor = ColorSensor(Port.S3)
# Calculate the light threshold. Choose values based on your measurements.
BLACK = 0
WHITE = 100
threshold = (BLACK + WHITE) / 2
# Set the drive speed at 100 millimeters per second.
DRIVE_SPEED = 201
# Set the gain of the proportional line controller. This means that for every
# percentage point of light deviating from the threshold, we set the turn
# rate of the drivebase to 1.2 degrees per second.
# For example, if the light value deviates from the threshold by 10, the robot
# steers at 10*1.2 = 12 degrees per second.
PROPORTIONAL_GAIN = 1.2
# Start following the line endlessly.
go_yes_or_no = True
while go_yes_or_no:
# 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)
print(robot.distance())
if robot.distance() >= distance:
go_yes_or_no = False
robot.stop()
robot.reset()
# You can wait for a short time or do other things in this loop.
wait(10)
def RedMission(): # Red Run (Bench Mission (including backrest removal))
# UP BUTTON
#!/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)
wheel_diameter = 56
axle_track = 115
robot = DriveBase(left_motor, right_motor, wheel_diameter, axle_track)
Medium_Motor = Motor(Port.A)
Large_Motor = Motor(Port.D)
leftcolorsensor = ColorSensor(Port.S3)
rightcolorsensor = ColorSensor(Port.S2)
robot.settings(500)
robot.straight(290)
robot.stop(Stop.BRAKE)
robot.settings(700,400,700,400)
robot.turn(110)
robot.stop(Stop.BRAKE)
while True:
robot.drive(200,0)
if leftcolorsensor.color() == Color.BLACK:
robot.stop(Stop.BRAKE)
break
robot.stop(Stop.BRAKE)
ev3.speaker.beep(3)
robot.turn(-110)
robot.straight(80)
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
robot.reset()
ev3.speaker.beep()
while True:
# Calculate the deviation from the threshold.
deviation = rightcolorsensor.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(rightcolorsensor.color())
if robot.distance() >= 100:
robot.stop(Stop.BRAKE)
break
robot.stop(Stop.BRAKE)
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
robot.reset()
while True:
# Calculate the deviation from the threshold.
deviation = rightcolorsensor.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(rightcolorsensor.color())
if leftcolorsensor.color() == Color.BLACK:
robot.stop(Stop.BRAKE)
break
robot.turn(-25)
robot.straight(230)
Large_Motor.run_angle(50,100,then = Stop.HOLD, wait = True)
robot.straight(-60)
robot.turn(35)
robot.straight(-10)
Large_Motor.run_angle(50,-50,then = Stop.HOLD, wait = True)
robot.straight(-85)
robot.turn(-85)
robot.straight(500)
robot.turn(-20)
robot.straight(250)
Large_Motor.run_angle(50,-70,then = Stop.HOLD, wait = False)
robot.turn(110)
robot.stop(Stop.BRAKE)
# PID tuning
Kp = 1 # proportional gain
Ki = 0 # integral gain
Kd = 0 # derivative gain
integral = 0
previous_error = 0
sp = 100
dt = 500 # milliseconds
done = False
collected = False
target_value = cs.reflection()
robot = DriveBase(lm, rm, 56, 114)
#Play a sound.
brick.sound.beep()
#Function Assignments
def slam(): #What you would expect.
pm.run_time(-400, 1000)
def forward(sp, t):
robot.drive(sp, t)
watch = StopWatch()
# Create your objects here.
ev3 = EV3Brick()
# Initialize the color sensor.
left_sensor = ColorSensor(Port.S1)
# Place on black.
ev3.speaker.say("Place on black")
# Write the value of the left color sensor to the screen.
while True:
# print("Button pressed: " + str(ev3.buttons.pressed()))
if (Button.CENTER in ev3.buttons.pressed()):
# Check the value of reflected light
reflected_light = left_sensor.reflection()
colorValues.log('black', reflected_light)
ev3.speaker.say("Recorded black")
break
# Place on white.
ev3.speaker.say("Place on white")
while True:
if (Button.CENTER in ev3.buttons.pressed()):
# Check the value of reflected light
reflected_light = left_sensor.reflection()
colorValues.log('white', reflected_light)
ev3.speaker.say("Recorded white")
break
def RedMission1():
#!/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)
wheel_diameter = 56
axle_track = 115
robot = DriveBase(left_motor, right_motor, wheel_diameter, axle_track)
Medium_Motor = Motor(Port.A)
Large_Motor = Motor(Port.D)
leftcolorsensor = ColorSensor(Port.S3)
rightcolorsensor = ColorSensor(Port.S2)
#####
BLACK = 9
WHITE = 85
threshold = (BLACK + WHITE) / 2
######
robot.straight(320)
robot.turn(110)
while True:
robot.drive(90, 0)
if leftcolorsensor.reflection() <= 9:
robot.stop(Stop.BRAKE)
break
robot.turn(-110)
robot.straight(200)
# Calculate the light threshold. Choose values based on your measurements.
BLACK = 6
WHITE = 85
threshold = (BLACK + WHITE) / 2
# Set the drive speed at 100 millimeters per second.
DRIVE_SPEED = 110
# Set the gain of the proportional line controller. This means that for every
# percentage point of light deviating from the threshold, we set the turn
# rate of the drivebase to 1.2 degrees per second.
# For example, if the light value deviates from the threshold by 10, the robot
# steers at 10*1.2 = 12 degrees per second.
PROPORTIONAL_GAIN = 1.2
runWhile = True
# Start following the line endlessly.
while runWhile:
# Calculate the deviation from the threshold.
deviation = rightcolorsensor.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)
if robot.distance() == 1000:
runWhile = False
# robot stops after finishing up line following code
robot.stop(Stop.BRAKE)
robot.straight(-40)
robot.turn(-50)
robot.straight(145)
Large_Motor.run_angle(50, 90, then=Stop.HOLD, wait=True)
#robot continues run, to do Boccia mission
while True:
robot.drive(-80, 0)
if leftcolorsensor.reflection() <= 10:
robot.stop(Stop.BRAKE)
break
robot.straight(80)
robot.turn(60)
robot.straight(100)
Large_Motor.run_angle(-50, 150, then=Stop.HOLD, wait=True)
robot.straight(-40)
Large_Motor.run_angle(50, 150, then=Stop.HOLD, wait=True)
robot.straight(-40)
while True:
robot.drive(-80, 0)
if leftcolorsensor.reflection() <= 9:
robot.stop(Stop.BRAKE)
break
robot.straight(40)
robot.turn(-85)
robot.straight(340)
robot.turn(165)
robot.straight(55)
Large_Motor.run_angle(-50, 150, then=Stop.HOLD, wait=True)
robot.straight(20)
Medium_Motor.run_angle(150, 250, then=Stop.HOLD, wait=True)
robot.turn(70)
Medium_Motor.run_angle(-150, 250, then=Stop.HOLD, wait=True)
robot.turn(-20)
robot.straight(-35)
Medium_Motor.run_angle(150, 250, then=Stop.HOLD, wait=True)
robot.turn(30)
robot.straight(-130)
def YellowMission():
#!/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 = 115
robot = DriveBase(left_motor, right_motor, wheel_diameter, axle_track)
# Initialize the color sensor.
line_sensor = ColorSensor(Port.S2)
line_sensor2 = ColorSensor(Port.S3)
robot.straight(110)
# Calculate the light threshold. Choose values based on your measurements.
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
# percentage point of light deviating from the threshold, we set the turn
# rate of the drivebase to 1.2 degrees per second.
# For example, if the light value deviates from the threshold by 10, the robot
# steers at 10*1.2 = 12 degrees per second.
PROPORTIONAL_GAIN = 1.2
runWhile = True
# Start following the line endlessly.
while runWhile:
# 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)
if robot.distance() == 800:
runWhile = False
robot.stop(Stop.BRAKE)
robot.drive(100, 0)
if line_sensor2 and cl.Color() == Color.BLACK:
robot.stop(Stop.BRAKE)
# robot stops after finishing up line following code
robot.stop(Stop.BRAKE)
# robot turns after finishing up line following code
robot.turn(-103.5)
# robot goes straight as it heads towards the mission
robot.straight(138)
# robot turns right for 90 degrees
robot.turn(80)
# robot goes straight towards the mission to line the attachment to the wheel
robot.straight(97)
# large motor attachment goes down to trap the wheel in
front_largeMotor.run_angle(60, 162)
# robot moves backwards to bring wheel outside of the large circle
robot.straight(-115)
# large motor releases the trapped tire
front_largeMotor.run_angle(60, -148)
# robot moves straight to get closer the wheel
robot.straight(38)
# robot turns so the wheel can get into the smaller target
robot.turn(-40)
robot.stop(Stop.BRAKE)
# robot goes backwards to leave the target and the wheel inside of it
robot.straight(-110)
# robot turns towards the weight machine
robot.turn(-30)
# going straight from row machine to weight machine
robot.straight(505)
# stopping for accuracy.
robot.stop(Stop.BRAKE)
# turning towards the weight machine.
robot.turn(30)
# robot goes straight to get closer to the weight machine
robot.straight(145)
# large motor going down to complete mission (weight machine).
front_largeMotor.run_angle(120, 130)
# going backwards away from the weight machine
robot.straight(-120)
# large motor goes back up
# front_largeMotor.run_angle(50, -100)
## The robot is turning away from the Weight Machine and towards the Boccia.
robot.turn(-127)
## The robot is moving straight towards the Boccia Mission.
robot.straight(290)
# the robot turns right to turn the aim boccia with the yellow axle on the bottom of the bot.
robot.turn(60)
# robot.straight(-10)
# robot.turn(15)
# front_largeMotor.run_angle(50, 60)
# robot.straight(55)
# the large motor goes up to push the yellow cube down into the target area.
front_largeMotor.run_angle(50, -50)
robot.straight(-100)
robot.turn(-45)
robot.straight(900)
robot.turn(25)
robot.straight(700)
class Robot:
def __init__(self):
# micro controller
ev3 = EV3Brick()
ev3.speaker.beep()
# sensors
# low value = registers black tape
# high value = aluminum
self.sensorYellow = ColorSensor(Port.S1)
self.sensorRed = ColorSensor(Port.S3)
self.sensorBlue = ColorSensor(Port.S2)
# motor
left_motor = Motor(Port.A, gears=[40, 24])
right_motor = Motor(Port.B, gears=[40, 24])
axle_track = 205
wheel_diameter = 35
self.motors = DriveBase(left_motor, right_motor,
wheel_diameter, axle_track)
# constants
# intersection detection of side sensors
self.thresholdBlueSensor = 30
# value for making turns
self.thresholdSideSensors = 15
# timer
self.watch = StopWatch()
self.timer = 0
def drive(self, directions):
i = 0
while i < len(directions):
self.timer = self.watch.time()
if self.timer % 100 == 0:
print(self.timer)
self.correctPath()
if self.senseIntersection() == True and self.timer >= 500:
print('intersection')
self.motors.drive_time(0, 0, 1000) # reduce this when done
self.executeCommand(directions[i])
i += 1
self.motors.drive(-125, 0)
def executeCommand(self, cmd):
if cmd == 0:
print('straight')
self.driveStraight()
if cmd == 1:
print('right')
self.turnRight()
if cmd == 2:
print('left')
self.turnLeft()
if cmd == 3:
print('reverse')
self.reverse()
if cmd == 4:
print('stop')
# turning behaviours at intersection
def turnLeft(self):
self.motors.drive_time(-30, 44, 2000)
self.watch.reset()
def turnRight(self):
self.motors.drive_time(-30, -46, 2000)
self.watch.reset()
def driveStraight(self):
self.motors.drive_time(-60, 0, 1800)
self.watch.reset()
def reverse(self):
self.motors.drive_time(60, 0, 1800)
self.motors.drive_time(0, 94, 2000)
self.motors.drive_time(-60, 0, 800)
# intersection detection
def senseIntersection(self):
if self.sensorRed.reflection() < 2 or self.sensorYellow.reflection() < 2:
return True
# path correction
# completely aluminum = 23
# completely black tape = 1
def correctPath(self):
if self.sensorBlue.reflection() < 8:
self.adjustLeft()
if self.sensorBlue.reflection() > 16:
self.adjustRight()
# default: -125, angle
def adjustLeft(self):
angle = 12 - min(self.sensorBlue.reflection(), 10)
step = 125 + (12 - min(self.sensorBlue.reflection(), 10))
self.motors.drive(-step, angle)
def adjustRight(self):
angle = max(self.sensorBlue.reflection(), 14) -12
step = 125 + (max(self.sensorBlue.reflection(), 14) -12)
self.motors.drive(-step, -angle)
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)
run_parallel(
self.be_noisy_to_people,
self.sting_by_ir_beacon,
self.pinch_if_touched,
self.keep_driving_by_ir_beacon)
class Robot:
def __init__(self):
"""Class that represents the robot
"""
try:
self.state = "Port 1: Right Color"
self.right_color = ColorSensor(Port.S1)
self.state = "Port 2: Center Color"
self.center_color = ColorSensor(Port.S2)
self.state = "Port 3: Left Color"
self.left_color = ColorSensor(Port.S3)
self.state = "Port 4: Gyro"
self.gyro = GyroSensor(Port.S4, Direction.COUNTERCLOCKWISE)
self.state = "Port A: Left Motor"
self.left_motor = Motor(Port.A)
self.state = "Port B: Right Motor"
self.right_motor = Motor(Port.B)
self.state = "Port C: Linear Gear"
self.linear_attachment_motor = Motor(Port.C)
self.state = "Port D: Block Dropper"
self.dropper_attachment_motor = Motor(Port.D)
self.wheel_diameter = 55
self.axle_track = 123
self.drive_base = DriveBase(self.left_motor, self.right_motor,
self.wheel_diameter, self.axle_track)
self.state = "OK"
self.clock = StopWatch()
self.dance_clock = 0
self.sign = 1
except:
brick.screen.clear()
big_font = Font(size=18)
brick.screen.set_font(big_font)
brick.screen.draw_text(0, 20, "Error!")
brick.screen.draw_text(0, 40, self.state)
def display_sensor_values(self):
"""Displays sensor values
"""
gyro_value = "Gyro : {}".format(self.gyro.angle())
left_color_value = "Left Color : {}".format(
self.left_color.reflection())
right_color_value = "Right Color : {}".format(
self.right_color.reflection())
center_color_value = "Center Color : {}".format(
self.center_color.reflection())
big_font = Font(size=18)
brick.screen.set_font(big_font)
brick.screen.clear()
brick.screen.draw_text(0, 20, gyro_value)
brick.screen.draw_text(0, 40, left_color_value)
brick.screen.draw_text(0, 60, right_color_value)
brick.screen.draw_text(0, 80, center_color_value)
def is_ok(self):
"""Tells if all sensors are plugged in
:return: Checks the state of the sensors
:rtype: Boolean
"""
return self.state == "OK"
def beep(self, is_down=False):
"""Plays a series of beeps
:param is_down: Tells if to play series downwards, defaults to False
:type is_down: bool, optional
"""
beep_counts = range(1, 7) if not is_down else range(6, 0, -1)
for beep_count in beep_counts:
brick.speaker.beep(400 * beep_count, 100)
wait(20)
def drift_check(self):
brick.speaker.beep(1200, 500)
wait(100)
brick.speaker.beep(1200, 500)
drift = False
start_gyro = self.gyro.angle()
brick.screen.clear()
big_font = Font(size=18)
brick.screen.set_font(big_font)
brick.screen.draw_text(0, 20, "Checking Gyro drift...")
wait(2000)
if start_gyro != self.gyro.angle():
brick.screen.draw_text(0, 60, "Error!")
brick.screen.draw_text(0, 80, "Gyro drift")
drift = True
return drift
def print_sensor_values(self):
"""Display robot sensor values. For debugging only
"""
print("Gyro: ", self.gyro.angle())
print("Left Color: ", self.left_color.reflection())
print("Right Color: ", self.right_color.reflection())
print("Center Color: ", self.center_color.reflection())
def stop_motors(self):
""" Stops all motors
"""
self.left_motor.stop(Stop.BRAKE)
self.right_motor.stop(Stop.BRAKE)
self.linear_attachment_motor.stop(Stop.BRAKE)
def drive(self, pid, speed, target_angle, duration):
"""Drives the robot using a gyro to a specific angle
:param pid: Uses Pid instance with parameters set beforehand
:type pid: Class
:param speed: Speed of the robot
:type speed: Number
:param target_angle: Angle to drive the robot at
:type target_angle: Number
:param duration: Duration the function is run
:type duration: Number
"""
# Inititialize values
pid.reset()
while pid.clock.time() < duration:
# Calculatr error
actual_angle = self.gyro.angle()
error = (target_angle - actual_angle) % 360
error = error - (360 * int(error / 180))
# Calculate steering output
steering = pid.compute_steering(error)
# Drive the motors
self.drive_base.drive(speed, steering)
self.print_sensor_values
# Stop motors
self.drive_base.stop(Stop.BRAKE)
def drive_dead_reckon(self, speed, duration, steering=0):
self.drive_base.drive(speed, steering)
wait(duration)
self.drive_base.stop(Stop.BRAKE)
def turn(self, pid, target_angle, tolerance=1):
"""Turns the robot to a specific angle.
:param pid: Uses Pid instance with parameters set beforehand
:type pid: Number
:param target_angle: Angle the robot turns to
:type target_angle: Number
:param tolerance: How close to the target angle you want the robot to be
:type tolerance: Number
"""
# Inititialize values
pid.reset()
error = tolerance + 1
min_speed = 50
while abs(error) > tolerance:
# Calculate error
actual_angle = self.gyro.angle()
error = (target_angle - actual_angle) % 360
error = error - (360 * int(error / 180))
# Call Pid compute_steering
steering = pid.compute_steering(error) * -1
# Set speed using a min_speed
right_speed = max(min_speed, abs(steering))
if steering < 0:
right_speed = -1 * right_speed
left_speed = -1 * right_speed
# Run motors
self.left_motor.run(left_speed)
self.right_motor.run(right_speed)
# Stop motors
self.left_motor.stop()
self.right_motor.stop()
def follow_line(self, pid, speed, duration, which_sensor, which_edge):
"""Follows the line using a color sensor.
:param pid: Uses Pid instance with parameters set beforehand
:type pid: Pid
:param speed: Speed of the Robot
:type speed: Number
:param duration: Duration of the function
:type duration: Number
:param which_sensor: The sensor the robot uses to follow the line
:type which_sensor: LineSensor
:param which_edge: Which side the white is on relative to the robot
:type which_edge: LineEdge
"""
# Inititialize values
pid.reset()
while pid.clock.time() < duration:
# Selecting which sensor to use using an Enum
if which_sensor == LineSensor.RIGHT:
error = 50 - self.right_color.reflection()
if which_sensor == LineSensor.LEFT:
error = 50 - self.left_color.reflection()
if which_sensor == LineSensor.CENTER:
error = 50 - self.center_color.reflection()
# Selecting which edge of the line to use
if which_edge == LineEdge.RIGHT:
pass
else:
error = error * -1
# Calculate steering
steering = pid.compute_steering(error)
# Run motors
self.drive_base.drive(speed, steering)
self.drive_base.stop(Stop.BRAKE)
def stop_on_white(self,
pid,
speed,
target_angle,
which_sensor,
color_value=80):
""" Gyro drives until given color sensor is on white
:param pid: PID setting of drive
:type pid: Number
:param speed: The speed the robot moves at
:type speed: Number
:param target_angle: the angle the gyro drives at
:type target_angle:
:param which_sensor: Chooses which color sensor to use
:type which_sensor: Enum
:param color_value: The value of color that the robot stops at
:type color_value: Number
"""
# Inititialize values
sensor = 0
pid.reset()
target_angle = target_angle % 360
if which_sensor == LineSensor.LEFT:
sensor = self.left_color
elif which_sensor == LineSensor.RIGHT:
sensor = self.right_color
else:
sensor = self.center_color
while sensor.reflection() < color_value:
# Calculate error
actual_angle = self.gyro.angle()
error = (target_angle - actual_angle) % 360
error = error - (360 * int(error / 180))
# Calculate steering output
steering = pid.compute_steering(error)
# Drive the motors
self.drive_base.drive(speed, steering)
self.print_sensor_values
# Stop motors
self.drive_base.stop(Stop.BRAKE)
def stop_on_black(self,
pid,
speed,
target_angle,
which_sensor,
color_value=15):
""" Gyro drives until given color sensor is on black
:param pid: PID setting of drive
:type pid: Number
:param speed: The speed the robot moves at
:type speed: Number
:param target_angle: the angle the gyro drives at
:type target_angle:
:param which_sensor: Chooses which color sensor to use
:type which_sensor: Enum
:param color_value: The value of color that the robot stops at
:type color_value: Number
"""
# Inititialize values
sensor = 0
pid.reset()
target_angle = target_angle % 360
if which_sensor == LineSensor.LEFT:
sensor = self.left_color
elif which_sensor == LineSensor.RIGHT:
sensor = self.right_color
else:
sensor = self.center_color
while sensor.reflection() > color_value:
# Calculate error
actual_angle = self.gyro.angle()
error = (target_angle - actual_angle) % 360
error = error - (360 * int(error / 180))
# Calculate steering output
steering = pid.compute_steering(error)
# Drive the motors
self.drive_base.drive(speed, steering)
self.print_sensor_values
# Stop motors
self.drive_base.stop(Stop.BRAKE)
def align(self, speed, sensor1, sensor2):
"""Aligns using color sensors on black line
:param speed: The speed the robot moves at
:type speed: Number
:param sensor1: The first sensor the robot uses to align
:type sensor1: Enum
:param sensor2: The second sensor the robot uses to align
:type sensor2: Enum
"""
self.left_motor.run(speed)
self.right_motor.run(speed)
first_sensor = 0
second_sensor = 0
if sensor1 == LineSensor.LEFT:
first_sensor = self.left_color
elif sensor1 == LineSensor.RIGHT:
first_sensor = self.right_color
else:
first_sensor = self.center_color
if sensor2 == LineSensor.LEFT:
second_sensor = self.left_color
elif sensor2 == LineSensor.RIGHT:
second_sensor = self.right_color
else:
second_sensor = self.center_color
while True:
first = False
second = False
if first_sensor.reflection() <= 10:
self.left_motor.hold()
first = True
if second_sensor.reflection() <= 10:
self.right_motor.hold()
second = True
if first and second == True:
break
def reset_sensors(self, reset_angle=0):
"""Reset the robot's sensor values
:param reset_angle: inital angle for the gyro, defaults to 0
:type reset_angle: int, optional
"""
# Resets the gyro
self.gyro.reset_angle(reset_angle)
def run_linear(self, speed, time, wait=True):
"""Runs linear gear
:param speed: The speed the linear gear moves at
:type speed: Number
:param time: How long the linear gear runs for
:type time: Number
:param wait: Wait for action to complete before next step
:type wait: Boolean
"""
self.stop_motors()
self.linear_attachment_motor.run_time(speed, time, Stop.BRAKE, wait)
def move_linear(self, speed, rotations, wait=True):
"""Will calculate the ratio of the gears and then move the linear gear
to a specific angle
:param speed: The speed the linear gear moves at
:type speed: Number
:param rotations: How much the linear gear moves by in rotations
:type rotations: Number
:param wait: Wait for action to complete before next step
:type wait: Boolean
"""
self.stop_motors()
target_angle = rotations * 360
self.linear_attachment_motor.run_angle(speed, target_angle, Stop.BRAKE,
wait)
def run_dropper(self, speed, time, wait=True):
"""Runs block dropper
:param speed: The speed the yeeter moves at
:type speed: Number
:param time: How long the yeeter runs for
:type time: Number
:param wait: Wait for action to complete before next step
:type wait: Boolean
"""
self.stop_motors()
self.dropper_attachment_motor.run_time(speed, time, Stop.BRAKE, wait)
def move_dropper(self, speed, degrees, wait=True):
"""Will calculate the ratio of the gears and then move the block dropper
to a specific angle
:param speed: The speed the yeeter moves at
:type speed: Number
:param degrees: How much the yeeter moves by in degrees
:type degrees: Number
:param wait: Wait for action to complete before next step
:type wait: Boolean
"""
self.stop_motors()
self.dropper_attachment_motor.run_angle(speed, degrees, Stop.BRAKE,
wait)
def dance(self, speed, duration):
if self.dance_clock == 0:
self.dance_clock = self.clock.time()
self.left_motor.run(speed * self.sign * -1)
self.right_motor.run(speed * self.sign)
if self.clock.time() - self.dance_clock > duration:
self.sign *= -1
self.left_motor.run(speed * self.sign * -1)
self.right_motor.run(speed * self.sign)
self.dance_clock = self.clock.time()
return True
return False
#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
#!/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
# Create your objects here.
ev3 = EV3Brick()
# Initialize the Ultrasonic Sensors.
obstacle_sensor = UltrasonicSensor(Port.S1)
color_sensor = ColorSensor(Port.S4)
# Initialize two motors.
left_motor = Motor(Port.B)
right_motor = Motor(Port.C)
arm_motor = Motor(Port.D)
# The DriveBase is composed of two motors, with a wheel on each motor.
robot = DriveBase(left_motor, right_motor, wheel_diameter=55.5, axle_track=104)
# Assemble Crew Code.
while color_sensor.reflection() < 50:
robot.drive(200, 0)
robot.stop(Stop.BRAKE)
# of the robot arm. It defines the starting point of the base.
base_switch = TouchSensor(Port.S1)
# Set up the Color Sensor. This sensor detects when the elbow
# is in the starting position. This is when the sensor sees the
# white beam up close.
elbow_sensor = ColorSensor(Port.S3)
# Initialize the elbow. First make it go down for one second.
# Then make it go upwards slowly (15 degrees per second) until
# the Color Sensor detects the white beam. Then reset the motor
# angle to make this the zero point. Finally, hold the motor
# in place so it does not move.
elbow_motor.run_time(-30, 1000)
elbow_motor.run(15)
while elbow_sensor.reflection() < 32:
wait(10)
elbow_motor.reset_angle(0)
elbow_motor.hold()
# 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
def BlueMission(): # Blue Run (Step Counter, Pull-Up Bar, Boccia Aim, Slide, Health Unit - 1)
# DOWN BUTTON
#!/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)
# Go towards the step counter mission from base
robot.settings(800) # Speed Change
robot.straight(650)
robot.stop(Stop.BRAKE)
wait(20)
# Slow the robot down to succesfully push the step counter.
robot.settings(200)
# Slowly pushes the step counter by going backward and forward a couple times to increase reliability.
robot.straight(230)
robot.straight(-20)
robot.straight(50)
robot.stop(Stop.BRAKE)
#robot.straight(-45)
#robot.stop(Stop.BRAKE)
#robot.straight(120)
#robot.stop(Stop.BRAKE)
robot.straight(-60)
robot.stop(Stop.BRAKE)
# The robot then turns and goes backwards until the right color sensor detects black.
#robot.settings(250,300,250,300)
robot.turn(45)
robot.straight(-100)
while True:
robot.drive(-100,0)
if line_sensor.color() == Color.BLACK:
robot.stop(Stop.BRAKE)
break
#The large motor attatchment comes down at the same time the robot takes a turn towards
#the black line underneath the pull up bar
left_motor.run_angle(50,-300,then=Stop.HOLD, wait=True)
# The robot then goes straight towards the line under the pull-up bar.
robot.straight(120)
robot.stop(Stop.BRAKE)
# Robot continues to go forwards until the left color sensor detects black.
while True:
robot.drive(115,0)
if line_sensor.color() == Color.BLACK:
robot.stop(Stop.BRAKE)
break
right_motor.run_angle(100,150,then=Stop.HOLD, wait=True)
# The robot turns using the right motor until it detects black.
while True:
right_motor.run(100)
if line_sensor.color() == Color.BLACK:
robot.stop(Stop.BRAKE)
break
robot.straight(-90)
large_motor.run_angle(100,150,then=Stop.HOLD, wait=True)
robot.stop(Stop.BRAKE)
robot.stop(Stop.BRAKE)
while True:
right_motor.run(40)
if line_sensor.color() == Color.BLACK:
robot.stop(Stop.BRAKE)
break
robot.stop(Stop.BRAKE)
ev3.speaker.beep()
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
robot.reset()
ev3.speaker.beep()
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(line_sensor1.color())
if line_sensor1.color() == Color.BLACK:
robot.stop(Stop.BRAKE)
break
robot.stop(Stop.BRAKE)
robot.stop(Stop.BRAKE)
large_motor.run_angle(-150, 150, then=Stop.HOLD, wait=False)
robot.turn(20)
robot.stop(Stop.BRAKE)
robot.settings(800)
robot.straight(280)
ev3.speaker.beep(3)
while True:
robot.drive(-115,0)
if line_sensor.color() == Color.BLACK:
ev3.speaker.beep(10)
robot.stop(Stop.BRAKE)
break
robot.stop(Stop.BRAKE)
# robot.straight(-10)
robot.stop(Stop.BRAKE)
robot.turn(50)
# left_motor.run_angle(100, 150)
'''
large_motor.run_angle(30,-20,then=Stop.HOLD, wait=False)
robot.turn(10)
robot.stop(Stop.BRAKE)
large_motor.run_angle(100, -50, then=Stop.HOLD, wait=False)
robot.turn(90)
robot.stop(Stop.BRAKE)
'''
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
robot.reset()
ev3.speaker.beep()
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(line_sensor.color())
if robot.distance() >= 500:
robot.stop(Stop.BRAKE)
break
ev3.speaker.beep(3)
while True:
robot.drive(40,0)
if line_sensor1.color() == Color.BLACK:
robot.stop(Stop.BRAKE)
break
robot.stop(Stop.BRAKE)
ev3.speaker.beep()
robot.straight(30)
robot.turn(-100)
robot.straight(70)
large_motor.run_angle(600,150,then=Stop.HOLD, wait=True)
while True:
robot.drive(-50, 0)
if line_sensor1.color() == Color.BLACK:
robot.stop(Stop.BRAKE)
break
robot.stop(Stop.BRAKE)
right_motor.run_angle(600,500,then=Stop.HOLD,wait=True)
robot.straight(20)
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
robot.reset()
ev3.speaker.beep()
while True:
# Calculate the deviation from the threshold.
deviation = line_sensor1.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(line_sensor1.color())
if robot.distance() >= 580:
robot.stop(Stop.BRAKE)
break
robot.stop(Stop.BRAKE)
while True:
robot.drive(50, 0)
if line_sensor.color() == Color.BLACK:
robot.stop(Stop.BRAKE)
break
robot.stop(Stop.BRAKE)
ev3.speaker.beep(3)
robot.turn(-45)
robot.stop(Stop.BRAKE)
robot.straight(30)
large_motor.run_angle(1000,-150,then=Stop.HOLD, wait=True)
robot.straight(-40)
large_motor.run_angle(1000,150,then=Stop.HOLD, wait=True)
robot.straight(40)
large_motor.run_angle(1000,-150,then=Stop.HOLD, wait=True)
robot.straight(-115)
robot.turn(95)
robot.straight(420)
robot.turn(-100)
robot.turn(100)
robot.turn(-100)
robot.turn(100)
robot.turn(-100)
robot.turn(100)
robot.turn(-100)
robot.turn(100)
robot.turn(-100)
robot.turn(100)
robot.turn(-100)
robot.turn(100)
robot.turn(-100)
robot.turn(100)
robot.turn(-100)
robot.turn(100)
robot.turn(-100)
robot.turn(100)
robot.turn(-100)
robot.turn(100)
robot.turn(-100)
robot.turn(100)
robot.turn(-100)
robot.turn(100)
robot.turn(-100)
robot.turn(100)
robot.turn(-100)
robot.turn(100)
robot.turn(-100)
robot.turn(100)
robot.turn(-100)
robot.turn(100)
def BlueMission():
#!/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)
#go front towards the step counter
robot.straight(650)
robot.stop(Stop.BRAKE)
wait(20)
#makes the robot go slower
robot.settings(40)
#slowly pushes the step counter by going back and front 2 times
robot.straight(140)
robot.stop(Stop.BRAKE)
robot.straight(-45)
robot.stop(Stop.BRAKE)
robot.straight(120)
robot.stop(Stop.BRAKE)
robot.settings(100)
robot.straight(-30)
robot.stop(Stop.BRAKE)
#the robot then turns and goes backwards
robot.turn(45)
robot.straight(-100)
# the robot then goes back until the right color sensor detects back
while True:
robot.drive(-30, 0)
if line_sensor.color() == Color.BLACK:
robot.stop(Stop.BRAKE)
break
#the large motor attatchment comes down at the same time the robot takes a turn towards the black line underneath the pull up bar
large_motor.run_angle(50, 170, then=Stop.HOLD, wait=False)
left_motor.run_angle(50, -300, then=Stop.HOLD, wait=True)
#the robot then goes straight towards that line
robot.straight(120)
robot.stop(Stop.BRAKE)
#robot continues to go forwards until the left color sensor detects black
while True:
robot.drive(30, 0)
if line_sensor.color() == Color.BLACK:
robot.stop(Stop.BRAKE)
break
right_motor.run_angle(50, 150, then=Stop.HOLD, wait=True)
#the robot then turns with the right motor until it detects black
while True:
right_motor.run(85)
if line_sensor.color() == Color.BLACK:
robot.stop(Stop.BRAKE)
break
#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
robot.reset()
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(line_sensor.color())
if line_sensor1.color() == Color.BLACK:
robot.stop(Stop.BRAKE)
break
#the robot then turns towards the boccia aim and moves straight to push it towards the target and finishes the misison
robot.straight(100) #after line following, it goes straight for 100 mm
robot.turn(50)
robot.straight(100)
robot.straight(-30)
large_motor.run_angle(100, -65)
robot.straight(-60)
#the robot then takes a turn (at the same time bringing the attatchment down) towards the slide mission and completes the mission
large_motor.run_angle(50, 80, then=Stop.HOLD, wait=False)
robot.turn(-195)
robot.straight(165)
large_motor.run_angle(300, -120, then=Stop.HOLD, wait=True)
robot.straight(-30)
large_motor.run_angle(200, 120, then=Stop.HOLD, wait=True)
## The robot moves straight towards the mission, getting ready to attempt to push the slide figures off once more. (In case it didn't work before.)
robot.straight(30)
large_motor.run_angle(300, -120, then=Stop.HOLD, wait=True)
robot.straight(-50)
'''
#htSide = Ev3devSensor(Port.S2) colLeft = ColorSensor(Port.S3) colRight = ColorSensor(Port.S4) except: pass stopwatch = StopWatch() LineTrack = PID_LineTrack(leftMotor, rightMotor, 0.3, 0, 5) Straight = PID_Straight(leftMotor, rightMotor, 0.5, 0, 10) # Write your program here. ev3.speaker.beep() watchtime = stopwatch.time() resetMotor(leftMotor, rightMotor) while rightMotor.angle() < 100: Straight.move(80)= stop(leftMotor, rightMotor) while colRight.reflection() > 10: LineTrack.track(80, 40) stop(leftMotor, rightMotor) resetMotor(leftMotor, rightMotor) while rightMotor.angle() < 200: Straight.move(80) stop(leftMotor, rightMotor)
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.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 wait, StopWatch, DataLog
from pybricks.robotics import DriveBase
import time
# 모터 선언
motorB = Motor(Port.B)
motorC = Motor(Port.C)
# 컬러 센서 선언
color_sensor = ColorSensor(Port.S1)
# 반사값과 모터의 속도는 원하는 값으로 수정할 수 있다.
# 단, 모터 속도가 너무 빠르면 반사값을 측정하는 주기가 길어져서 정확도가 낮아진다.
while True:
if color_sensor.reflection() < 30: # 반사값이 30보다 작다면,
motorB.run(150) # 모터 B를 150의 속도로 동작하고
motorC.run(0) # 모터 C를 정지한다
else: # 반사값이 30 이상이라면,
motorB.run(0) # 모터 B를 정지하고
motorC.run(150) # 모터 C를 150의 속도로 동작한다
def BlueMission():
#!/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)
#go front towards the step counter
robot.settings(250)
robot.straight(650)
robot.stop(Stop.BRAKE)
wait(20)
#makes the robot go slower
robot.settings(40)
#slowly pushes the step counter by going back and front 2 times
robot.straight(140)
robot.stop(Stop.BRAKE)
robot.straight(-45)
robot.stop(Stop.BRAKE)
robot.straight(120)
robot.stop(Stop.BRAKE)
robot.straight(-30)
robot.stop(Stop.BRAKE)
#the robot then turns and goes backwards
robot.settings(250,500,250,500)
robot.turn(45)
robot.straight(-100)
# the robot then goes back until the right color sensor detects back
while True:
robot.drive(-115,0)
if line_sensor.color() == Color.BLACK:
robot.stop(Stop.BRAKE)
break
#the large motor attatchment comes down at the same time the robot takes a turn towards the black line underneath the pull up bar
large_motor.run_angle(50,200,then=Stop.HOLD, wait=False)
left_motor.run_angle(50,-300,then=Stop.HOLD, wait=True)
#the robot then goes straight towards that line
robot.straight(120)
robot.stop(Stop.BRAKE)
#robot continues to go forwards until the left color sensor detects black
while True:
robot.drive(115,0)
if line_sensor.color() == Color.BLACK:
robot.stop(Stop.BRAKE)
break
right_motor.run_angle(50,150,then=Stop.HOLD, wait=True)
#the robot then turns with the right motor until it detects black
while True:
right_motor.run(85)
if line_sensor.color() == Color.BLACK:
robot.stop(Stop.BRAKE)
break
#follows the line underneath the pull up bar until the leftsensor detects black
#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
robot.reset()
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(line_sensor1.color())
if line_sensor1.color() == Color.BLACK:
robot.stop(Stop.BRAKE)
break
#the robot then turns towards the boccia aim and moves straight to push it towards the target and finishes the misison
robot.turn(25)
robot.straight(250)
robot.straight(-50)
# this is importate kekekekee
large_motor.run_angle(75,-65)
robot.straight(-60)
while True:
robot.drive(-70,0)
if line_sensor.color() == Color.BLACK:
robot.stop(Stop.BRAKE)
ev3.speaker.beep()
break
while True:
left_motor.run(-50)
if line_sensor1.color() == Color.BLACK:
robot.stop(Stop.BRAKE)
ev3.speaker.beep()
break
left_motor.run_angle(50, -20)
right_motor.run_angle(50, 20)
robot.settings(200)
robot.straight(60)
robot.turn(-137)
#this is also importante jekeke
large_motor.run_angle(50,80)
robot.straight(143)
large_motor.run_angle(550, -120)
robot.straight(-40)
large_motor.run_angle(550, 120)
robot.straight(40)
large_motor.run_angle(550, -120)
large_motor.run_angle(300, 30, then=Stop.HOLD, wait=True)
#robot.straight(40)
large_motor.run_angle(300, -100, then=Stop.HOLD, wait=True)
#goes to collect the health unit near the basketball (goes back to base)
robot.straight(-200)
robot.turn(40)
robot.straight(556)
robot.straight(50)
robot.stop(Stop.BRAKE)
while True:
left_motor.run(50)
if line_sensor1.color() == Color.BLACK:
robot.stop(Stop.BRAKE)
ev3.speaker.beep()
break
robot.stop(Stop.BRAKE)
robot.reset()
# Calculate the light threshold. Choose values based on your measurements.
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
# percentage point of light deviating from the threshold, we set the turn
# rate of the drivebase to 1.2 degrees per second.
# For example, if the light value deviates from the threshold by 10, the robot
# steers at 10*1.2 = 12 degrees per second.
PROPORTIONAL_GAIN = 1.2
runWhile = True
# Start following the line endlessly.
while runWhile:
# Calculate the deviation from the threshold.
deviation = line_sensor1.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)
if robot.distance() >= 210:
runWhile = False
break
robot.stop(Stop.BRAKE)
robot.turn(5.244312)
robot.straight(700)
#the robot then goes back until the right color sensor detects back
'''
while True:
if line_sensor1.color() == Color.BLACK:
robot.stop(Stop.BRAKE)
break
#robot.straight(980)
'''
robot.stop(Stop.BRAKE)
import time
# 모터 선언
motorB = Motor(Port.B)
motorC = Motor(Port.C)
# 컬러 센서 선언
color_sensor1 = ColorSensor(Port.S1)
color_sensor2 = ColorSensor(Port.S2)
CRITERIA = 20 # 반사값 기준 설정
speed = 150 # 모터 속도
while True:
if color_sensor1.reflection() > CRITERIA and color_sensor2.reflection(
) > CRITERIA:
# 센서 2개 모두 선 위에 있지 않을 경우, 직진한다.
motorB.run(speed)
motorC.run(speed)
elif color_sensor1.reflection() > CRITERIA and color_sensor2.reflection(
) <= CRITERIA:
# 오른쪽 센서 1개만 선 위에 올라갔을 경우, 오른쪽으로 회전한다.
motorB.run(speed)
motorC.run(0)
elif color_sensor1.reflection() <= CRITERIA and color_sensor2.reflection(
) > CRITERIA:
# 왼쪽 센서 1개만 선 위에 올라갔을 경우, 왼쪽으로 회전한다.
motorB.run(0)
def adjustRight():
while sensorRed.reflection() < 30 or sensorBlue.reflection() > 30:
motors.drive_time(0, 30, 200)
def sensorCheck():
print()
print("yellow: " + str(sensorYellow.reflection()))
print("red: " + str(sensorRed.reflection()))
print("blue: " + str(sensorBlue.reflection()))
while True:
while sensorBlue.reflection() < 15: # Is motors on a path
motors.drive(200, 0)
# T intersection
if sensorRed.reflection() < 15 and sensorYellow.reflection() < 15:
sensorCheck()
print('crossroads')
choice = path[i]
#choice = random.randrange(0, 3)
if choice == 0:
print('straight')
motors.drive_time(speed_d, 0, speed_t)
i += 1
if choice == 1:
print('right')
turnRight()
