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
import math
# Write your program here
brick.sound.beep()
gyro = GyroSensor(Port.S2)
motor1 = Motor(Port.B)
motor2 = Motor(Port.C)
robot = DriveBase(motor1, motor2, 56, 50)
gyro.mode = "GYRO-CAL"
gyro.reset_angle(0)
#Must start robot in upright balanced position
while True:
angle = gyro.speed()
print(angle)
#This part actually implements
if angle > 3:
robot.drive_time(5*angle+100,0, 200)
elif angle < -3:
robot.drive_time(5*angle-100,0,200)
else:
robot.stop(Stop.HOLD)
# Initialize the EV3 Brick
ev3 = EV3Brick()
# Set volume to 100% and make a beep to signify program has started
ev3.speaker.set_volume(100)
ev3.speaker.beep()
# Initialize EV3 touch sensor and motors
motor = Motor(Port.A)
gyro = GyroSensor(Port.S1)
# Hold the gyro sensor still. THe program will display the current gyro
# speed. If the speed is not close to zero unplug the gyro sensor from the
# EV3 brick, hold it still and plug is back in.
print("Speed: ", gyro.speed())
# Create a loop to react to buttons
while True:
# Check for center button events
if Button.CENTER in ev3.buttons.pressed():
motor.stop()
break
# Rotate the gyro sensor using the red dot as the center. The angle will
# range from -90 to 90. This is close to perfect as the motor dc method
# requires a value from -100 to 100.
motor.dc(gyro.angle())
wait(10)
class Robot:
def __init__(self, left_motor_port, right_motor_port, med_motor_port,
gyro_port, wheel_diameter, wheel_base):
self.left_motor = Motor(left_motor_port)
self.right_motor = Motor(right_motor_port)
# self.med_motor = Motor(med_motor_port)
self.robot = DriveBase(self.left_motor, self.right_motor,
wheel_diameter, wheel_base)
self.gyro = GyroSensor(gyro_port)
# Function definitions
# Gyro sensor reset, waits until the gyro has come to rest and then resets the value to zero
# use at beginning of mission programs
def resetGyro(self):
brick.light(Color.RED)
speed = self.gyro.speed()
angle = self.gyro.angle()
while speed != 0:
wait(100)
speed = self.gyro.speed()
angle = self.gyro.angle()
self.gyro.reset_angle(0)
brick.light(Color.GREEN)
# Drive the robot straight using the GyroSensor
#
def driveStraight(self, rotations, speed):
distance = rotations * 360 # convert wheel rotations to degrees
self.gyro.reset_angle(0)
self.left_motor.reset_angle(0)
self.right_motor.reset_angle(0)
# set our amount to correct back towards straight
correction = -2
if speed < 0:
correction = 2
# start the robot driving
self.robot.drive(speed, 0)
# loop until the robot has travelled the distance we want
# updating the steering angle of the drive based on the gyro measured drift and correction
while abs(self.left_motor.angle()) <= distance and abs(
self.right_motor.angle()) <= distance:
drift = self.gyro.angle()
print("Drift: " + str(drift))
steering = drift * correction
#print("Steering: " + str(steering))
self.robot.drive(speed, steering)
self.robot.stop(Stop.BRAKE)
# Turn the robot an exact amount using the GryoSensor
def turnDegrees(self, degrees):
self.gyro.reset_angle(0)
initial_power = 300
end_power = 50
left_motor_power = initial_power
right_motor_power = initial_power * -1
if degrees < 0:
left_motor_power = initial_power * -1
right_motor_power = initial_power
initial_turn = abs(degrees * .75)
self.left_motor.run(left_motor_power)
self.right_motor.run(right_motor_power)
angle = self.gyro.angle()
print("Angle: " + str(angle))
while abs(angle) < initial_turn:
wait(10)
angle = self.gyro.angle()
print("Angle: " + str(angle))
left_motor_power = end_power
right_motor_power = end_power * -1
if degrees < 0:
left_motor_power = end_power * -1
right_motor_power = end_power
self.left_motor.run(left_motor_power)
self.right_motor.run(right_motor_power)
end_degrees = (abs(degrees) - 1)
angle = self.gyro.angle()
print("Angle: " + str(angle))
while abs(angle) < end_degrees:
wait(10)
angle = self.gyro.angle()
print("Angle: " + str(angle))
self.left_motor.stop(Stop.BRAKE)
self.right_motor.stop(Stop.BRAKE)
print("Final Angle: " + str(self.gyro.angle()))
def oldDrive(self, speed, turn):
while True:
self.robot.drive(speed, turn)
# plug in
# S1 = touch sensor
# S2 = light sensor
# S3 = gyro sensor
# S4 = ultrasonic sensor
touch = TouchSensor(Port.S1)
while not touch.pressed():
wait (10)
light = ColorSensor(Port.S2)
light.rgb()
wait(100)
light.color()
# returns Color.BLACK, Color.BLUE, Color.GREEN, Color.YELLOW, Color.RED, Color.WHITE, Color.BROWN or None.
wait(100)
while not (light.color() == Color.YELLOW):
wait(100)
light.reflection()
wait(100)
light.ambient()
wait(100)
gyro = GyroSensor(Port.S3)
gyro.reset_angle(0) # define your starting angle
while gyro.angle() < 90:
wait(100)
gyro.speed()
class Robot():
def __init__(self):
self.brick = EV3Brick()
self.frontMotor = Motor(Port.D)
self.rearMotor = Motor(Port.A)
self.leftMotor = Motor(Port.C)
self.rightMotor = Motor(Port.B)
if path.exists('sensorpoints.py'):
import sensorpoints
self.leftSensor = LightSensor(Port.S3, sensorpoints.leftLow,
sensorpoints.leftHigh)
self.rightSensor = LightSensor(Port.S2, sensorpoints.rightLow,
sensorpoints.rightHigh)
else:
self.leftSensor = LightSensor(Port.S3, 10, 105)
self.rightSensor = LightSensor(Port.S2, 20, 160)
self.gyroSensor = GyroSensor(Port.S1)
wait(100)
self.gyroSensor.speed()
self.gyroSensor.angle()
wait(500)
self.gyroSensor.reset_angle(0.0)
wait(200)
def calibrate(self):
rightHigh = 40
rightLow = 70
leftHigh = 40
leftLow = 70
timer = StopWatch()
self.moveSteering(0, 125)
while timer.time() < 5000:
if self.rightSensor.light() > rightHigh:
rightHigh = self.rightSensor.light()
if self.rightSensor.light() < rightLow:
rightLow = self.rightSensor.light()
if self.leftSensor.light() > leftHigh:
leftHigh = self.leftSensor.light()
if self.leftSensor.light() < leftLow:
leftLow = self.leftSensor.light()
self.stop()
# write results to file
with open('sensorpoints.py', 'w') as myFile:
myFile.write('leftLow = ')
myFile.write(str(leftLow))
myFile.write('\nrightLow = ')
myFile.write(str(rightLow))
myFile.write('\nleftHigh = ')
myFile.write(str(leftHigh))
myFile.write('\nrightHigh = ')
myFile.write(str(rightHigh))
def moveSteering(self, steering, speed):
leftMotorSpeed = speed * min(1, 0.02 * steering + 1)
rightMotorSpeed = speed * min(1, -0.02 * steering + 1)
self.leftMotor.run(leftMotorSpeed)
self.rightMotor.run(rightMotorSpeed)
def drive(self, distance, speed, time=8, kSteering=1):
# Startup for gyro
#kSteering=5 # Steering coefficient
startDegrees = self.gyroSensor.angle()
# Startup for ramp speed
if distance < 0:
distance = abs(distance)
speed = -speed
rotation = distance * 51.9
self.rightMotor.reset_angle(0)
# Loop
while abs(self.rightMotor.angle()) < abs(rotation):
# Do the gyro steering stuff
currentDegrees = self.gyroSensor.angle()
errorGyro = currentDegrees - startDegrees
# Do the ramp speed stuff
rampSpeed = min(
sin(abs(self.rightMotor.angle()) / rotation * 3.14),
abs(speed) - 100)
self.moveSteering(errorGyro * kSteering * copysign(1, speed),
rampSpeed * speed + copysign(100, speed))
# Exit
self.stop()
def turn(self, angle, speed, time=5):
# Startup
steering = 100
kTurn = 0.01
offset = 20
timer = StopWatch()
# Loop
while (abs(self.gyroSensor.angle() - angle) > 0) & (timer.time() <
time * 1000):
error = self.gyroSensor.angle() - angle
#if error > 0 :
# steering = 100
#else:
# steering = -100
self.moveSteering(steering,
speed * error * kTurn + copysign(offset, error))
# Exit
self.stop(Stop.HOLD)
print("turning to: ", angle, " gyro: ", self.gyroSensor.angle())
def lineFollow2Line(self, speed, rightSide=True, rightFollow=True):
# Startup
if rightFollow:
followSensor = self.rightSensor
stopSensor = self.leftSensor
else:
followSensor = self.leftSensor
stopSensor = self.rightSensor
if rightSide:
kSide = 1
else:
kSide = -1
lastError = 0
# Loop
while not stopSensor.isWhite():
error = followSensor.line - followSensor.light()
pCorrection = error * 0.25
dError = lastError - error
dCorrection = dError * 1.25
self.moveSteering((pCorrection - dCorrection) * kSide, speed)
lastError = error
self.stop()
def lineFollow4Time(self, speed, time, rightSide=True, rightFollow=True):
# Startup
if rightFollow:
followSensor = self.rightSensor
else:
followSensor = self.leftSensor
if rightSide:
kSide = 1
else:
kSide = -1
timer = StopWatch()
lastError = 0
# Loop
while timer.time() < time * 1000:
# Experimental settings: kp = 0.2, kd = 0.4
error = followSensor.line - followSensor.light()
pCorrection = error * 0.25 # Used to be 0.25
dError = lastError - error
dCorrection = dError * 1.2 # Used to be 1.25
self.moveSteering((pCorrection - dCorrection) * kSide, speed)
lastError = error
#wait(10)
self.stop()
def turn2Line(self, speed, rightStop=True, time=5):
if rightStop:
stopSensor = self.rightSensor
else:
stopSensor = self.leftSensor
self.moveSteering(100, speed)
stopSensor.waitForLine()
self.stop()
def drive2Line(self, speed, distanceBefore, distanceAfter, rightStop=True):
# Startup
if rightStop:
stopSensor = self.rightSensor
else:
stopSensor = self.leftSensor
self.drive(distanceBefore, speed)
# Loop
#while self.rightSensor.rgb() < 90:
# self.moveSteering(0, 70)
# Start Driving
# *** OLD ONE *** self.moveSteering(0, 250)
self.moveSteering(0, 0.5 * speed)
# execute function wait until we detect a line
stopSensor.waitForLine()
self.stop(Stop.HOLD)
# Exit
self.drive(distanceAfter, speed)
def stop(self, brakeType=Stop.HOLD):
# 3 options: Stop.BRAKE, Stop.COAST, Stop.HOLD
self.rightMotor.stop(brakeType)
self.leftMotor.stop(brakeType)
def wait4Button(self):
self.brick.speaker.beep()
while not any(self.brick.buttons.pressed()):
pass
def gyroSet(self, newAngle=0):
#while not self.gyroCheck():
# pass
startAngle = self.gyroSensor.angle()
wait(100)
#self.gyroSensor.speed()
#self.gyroSensor.angle()
wait(500)
self.gyroSensor.reset_angle(newAngle)
wait(500)
print("Gyro Start: ", startAngle, "Gyro Reset. Goal: ", newAngle,
" Actual: ", self.gyroSensor.angle())
def gyroCheck(self):
angle1 = self.gyroSensor.angle()
wait(1000)
if self.gyroSensor.angle() != angle1:
print("drift detected")
self.brick.speaker.say("grace forgot her necklace")
return False
else:
print("absence of drift")
self.brick.speaker.say("grace remebered her necklace")
return True
# Initialize the EV3 brick.
ev3 = EV3Brick()
# Initialize the gyro sensor. It is used to provide feedback for balancing the
# robot.
gyro_sensor = GyroSensor(Port.S2)
# The following (UPPERCASE names) are constants that control how the program
# behaves.
GYRO_CALIBRATION_LOOP_COUNT = 200
GYRO_OFFSET_FACTOR = 0.0005
# Calibrate the gyro offset. This makes sure that the robot is perfectly
# still by making sure that the measured rate does not fluctuate more than
# 2 deg/s. Gyro drift can cause the rate to be non-zero even when the robot
# is not moving, so we save that value for use later.
while True:
gyro_minimum_rate, gyro_maximum_rate = 440, -440
gyro_sum = 0
for _ in range(GYRO_CALIBRATION_LOOP_COUNT):
gyro_sensor_value = gyro_sensor.speed()
gyro_sum += gyro_sensor_value
if gyro_sensor_value > gyro_maximum_rate:
gyro_maximum_rate = gyro_sensor_value
if gyro_sensor_value < gyro_minimum_rate:
gyro_minimum_rate = gyro_sensor_value
wait(5)
if gyro_maximum_rate - gyro_minimum_rate < 2:
break
gyro_offset = gyro_sum / GYRO_CALIBRATION_LOOP_COUNT
