def rotate_180_by_gyro(self):
gyro_sensor = GyroSensor(Port.S4)
gyro_sensor.reset_angle(0)
self.left_motor.run(self.TRUCK_SPEED)
self.right_motor.run(-self.TRUCK_SPEED)
while (gyro_sensor.angle() < 135):
print("Angle: ", gyro_sensor.angle())
wait(1)
self.stop()
wait(4000)
def run():
motor_b = Motor(Port.B)
motor_c = Motor(Port.C)
Gyro = GyroSensor(Port.S1)
Gyro.reset_angle(0)
motor_b.run_angle(500, 720, Stop.BRAKE, False)
motor_c.run_angle(500, 720, Stop.BRAKE, True)
wait(10)
while Gyro.angle() <= 180:
motor_c.run(100)
motor_b.run(-100)
wait(10)
motor_b.run_angle(500, 720, Stop.BRAKE, False)
motor_c.run_angle(500, 720, Stop.BRAKE, True)
def init_brick():
# Create your objects here.
ev3 = EV3Brick()
# Initilize our motors
left_motor = Motor(Port.A)
right_motor = Motor(Port.D)
front_motor_1 = Motor(Port.C)
front_motor_2 = Motor(Port.B)
left_motor.reset_angle(0)
right_motor.reset_angle(0)
front_motor_1.reset_angle(0)
front_motor_2.reset_angle(0)
# Initialize the color sensor.
left_sensor = ColorSensor(Port.S4)
right_sensor = ColorSensor(Port.S1)
# Speeds
right_sensor = ColorSensor(Port.S1)
left_sensor = ColorSensor(Port.S4)
ARM_MOTOR_SPEED = 400
WHEEL_DIAMETER = 92
AXLE_TRACK = 130
DRIVE_SPEED_FAST = 350
DRIVE_SPEED_NORMAL = 200
DRIVE_SPEED_SLOW = 100
DRIVE_EXTRA_SLOW = 30
CIRCUMFERENCE = 3.14 * WHEEL_DIAMETER # Diameter = 100mm, Circumference = 314.10mm = 1 rotation
# Initialize the Gyro sensor
gyro = GyroSensor(Port.S2)
gyro.reset_angle(0)
# All parameters are in millimeters
robot = DriveBase(left_motor,
right_motor,
wheel_diameter=config.WHEEL_DIAMETER,
axle_track=config.AXLE_TRACK)
# Set the straight speed and turn rate
robot.settings(straight_speed=config.DRIVE_SPEED_NORMAL,
turn_rate=config.TURN_RATE)
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
class Robot:
"""Classe do Robo"""
"""Todos os métodos tem como primeiro parâmetro a palavra-chave 'self' e é ela que faz a referência
a outras variáveis e métodos dentro da classe 'Robot'.
Os outros parametros funcionam da mesma forma que em funcoes normais.
"""
def __init__(self, portaMotorEsquerdo, portaMotorDireito):
"""Esse metodo é a função de inicialização de um novo dado do tipo 'Robot'.
Podemos dizer, então, que é o método de inicialização de um novo objeto da classe 'Robot'.
Passamos os parametros:
'self', por ser um método, e as portas que serão associadas aos elementos do robo:"""
self.motorEsquerdo = Motor(portaMotorEsquerdo)
self.motorDireito = Motor(portaMotorDireito)
self.sensor = None # Como nao sabemos qual tipo de sensor será conectado, vamos deixar a variável <sensor> sem nenhum valor por enquanto
self.tipoSensor = "nenhum" # Podemos usar essa string para verificar qual o tipo de sensor conectado antes de utilizar ele no codigo
""" Cada tipo de sensor diferente que pode ser conectado tem uma inicialização diferente
Por isso podemos definir uma inicialização para cada tipo que poderemos utilizar
Se soubessemos exatamente quais sensores estariam no robo, eles poderiam ser inicializados direto no metodo <__init__>,
assim como foram os motores.
Nesse caso, nao seria necessario verificar o tipo de sensor, pois sempre saberiamos qual o tipo e de que forma utiliza-lo."""
def iniciaSensorCor(self, portaSensor): # Para o sensor de cor
self.sensor = ColorSensor(portaSensor)
self.tipoSensor = "cor"
def iniciaSensorUltra(self, portaSensor): # Para o sensor ultrassonico
self.sensor = UltrasonicSensor(portaSensor)
self.tipoSensor = "ultra"
def iniciaSensorInfra(self, portaSensor): # Para o sensor infravermelho
self.sensor = InfraredSensor(portaSensor)
self.tipoSensor = "infra"
def iniciaSensorGiro(self, portaSensor): # Para o sensor giroscopio
self.sensor = GyroSensor(portaSensor)
self.tipoSensor = "giro"
"""Metodos para utilizacao dos recursos do robo:"""
def andarTempo(self, velocEsquerda, velocDireita, tempo):
cronometro = StopWatch() # Definimos um cronometro
cronometro.reset() # Resetamos o tempo marcado no cronometro
while cronometro.time() < tempo:
self.motorDireito.run(velocEsquerda)
self.motorEsquerdo.run(velocDireita)
self.motorDireito.stop()
self.motorEsquerdo.stop()
def andarRetoGraus(self, veloc, graus):
while (self.motorEsquerdo.angle() < graus) and (self.motorDireito.angle() < graus):
self.motorDireito.run(veloc)
self.motorEsquerdo.run(veloc)
self.motorDireito.stop()
self.motorEsquerdo.stop()
def curvaGiro(self, veloc, graus): # Curva com os dois motores utilizando o giroscopio
if self.tipoSensor != "giro": # Verifica se o sensor do tipo certo esta conectado
print("ERRO: GIROSCOPIO NAO CONECTADO.")
return False # Interrompe o metodo
self.sensor.reset_angle(0)
while self.sensor.angle() < graus:
self.motorDireito.run(-veloc)
self.motorEsquerdo.run(veloc)
self.motorDireito.stop()
self.motorEsquerdo.stop()
def andaAteObstaculo(self, veloc, distancia):
if self.tipoSensor != "ultra" and self.tipoSensor != "infra": # O mesmo codigo funciona pro ultrassonico e pro infravermelho
print("ERRO: SENSOR DE DISTANCIA NAO CONECTADO")
return False # Interrompe o metodo
while self.sensor.distance() < distancia:
self.motorEsquerdo.run(veloc)
self.motorDireito.run(veloc)
self.motorEsquerdo.stop()
self.motorDireito.stop()
def andaAteCor(self, veloc, cor):
if self.tipoSensor != "cor":
print("ERRO: SENSOR DE COR NAO CONECTADO")
return False # Interrompe o metodo
while self.sensor.color() != cor:
self.motorEsquerdo.run(veloc)
self.motorDireito.run(veloc)
self.motorEsquerdo.stop()
self.motorDireito.stop()
def angPoligono(self, lados):
if lados <= 2 :
print("NAO EH UM POLIGONO")
return False
if self.tipoSensor != "giro":
print("ERRO: SENSOR GIROSCOPIO NAO CONECTADO")
return False # Interrompe o metodo
angint = (((lados - 2)*180)/lados)
#calculo para angulos internos de um polígono"
return angint
#!/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
esquerda = Motor(Port.B, Direction.COUNTERCLOCKWISE)
direita = Motor(Port.C, Direction.COUNTERCLOCKWISE)
MotorMA = Motor(Port.A)
MotorMD = Motor(Port.D)
melhorSensor = GyroSensor(Port.S2)
ev3 = EV3Brick
while True:
if Button.CENTER in ev3.buttons.pressed():
a = melhorSensor.angle()
ev3.screen.print(a)
if Button.LEFT in ev3.buttons.pressed():
melhorSensor.reset_angle(0)
from pybricks.robotics import DriveBase
UltrasonicSensor = UltrasonicSensor(Port.S1) #Definieren des Ultraschallsensors
leftcolorsensor = ColorSensor(Port.S2) #Definieren des linken Farbsensors
rightcolorsensor = ColorSensor(Port.S3) #Definieren des rechten Farbsensors
gyro = GyroSensor(Port.s4) #Definieren des Gyrosensors
rightMotor = Motor(Port.A) #Definition des rechten Motors
leftMotor = Motor(Port.B) #Definition des linken Motors
watch=StopWatch() #Definieren der Stoppuhr
navi=[] #Einbauen einer list/eines Speichers
V=300 #Definieren der Geschwindigkeit
gyro.reset_angle(0) #Setzen des Winkelwertes auf 0
rot = gyro.angle() #Messen des Winkelwertes
while abs(rot)<85: #Solange der Wert der Rotation kleiner ist als 85,
seconds = watch.time()/1000 #Zähle Zeit in Sekunden
print(seconds)
leftMotor.run(V) #Beide Motoren fahren vorwärts
rightMotor.run(V)
rot = gyro.angle() #Messen des Winkelwerts
if abs(rot)>=85: #Sobald Der Wert der Rotation größer oder gleich 85 ist,
navi.append(seconds) #Fügt dem Speicher "Navi" die aktuelle Zeit in Sekunden ein
seconds=0 #Setzt die Sekunden auf 0
seconds = watch.time()/1000 #Zählt die Zeit in Sekunden
# 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()
"""
from pybricks.ev3devices import Motor, ColorSensor, GyroSensor
from pybricks.parameters import Port , Direction
from pybricks.tools import wait
from pybricks.robotics import DriveBase
import Dimensions
# Initialize the motors.
left_motor = Motor(Port.B , positive_direction=Direction.COUNTERCLOCKWISE, gears=[40,8])
right_motor = Motor(Port.C , positive_direction=Direction.COUNTERCLOCKWISE, gears=[40,8])
gyro_sensor = GyroSensor(Port.S3)
# Initialize the color sensor.
#line_sensor = ColorSensor(Port.S3)
left_motor.reset_angle(0)
right_motor.reset_angle(0)
gyro_sensor.reset_angle(0)
fudge=1
speed=100
angle=0
robot = DriveBase(left_motor, right_motor, wheel_diameter=30, axle_track=135)
initial_distance = 0
robot.reset()
while ((robot.distance() - initial_distance) < 350) :
robot.drive(speed,angle)
drift = gyro_sensor.angle()
angle = (drift * fudge) * -1
wait(3)
i=0
mC = Motor(Port.C) robot = DriveBase(mB, mC, 94, 115) # wheel OD=94mm, wheelbase=115mm mB.set_run_settings(200, 250) # max speed, max accel mC.set_run_settings(200, 250) # max speed, max accel gy = GyroSensor(Port.S3) gy2 = GyroSensor(Port.S2) gy.mode='GYRO-RATE' # deliver turn-rate (must integrate for angle) gy2.mode='GYRO-RATE' # deliver turn-rate (must integrate for angle) sleep(0.5) gy.mode='GYRO-ANG' # changing modes causes recalibration gy2.mode='GYRO-ANG' # changing modes causes recalibration sleep(3) gy.reset_angle(0) gy2.reset_angle(0) mB.reset_angle(0) mC.reset_angle(0) angle = 0 # global var holding accumulated turn angle runB = False # whether Motor B should be running right now runC = False t = Thread(target=printAngle) tstart = watch.time() / 1000 t.start() # start angle monitor routine brick.sound.beep(400, 10) # initialization-complete sound
) #Suchen nach schwarzer Linie links vom Roboter
if (color1 == Color.BLACK): #Wenn eine schwarze Linie zu sehen ist,
brick.display.text("Drehe n. Links",
(0, 50)) #Wird dieser Text angezeigt
leftMotor.run(200) #der linke Motor fährt
rightMotor.run(100) #der rechte Motor fährt langsamer als der Linke
color2 = rightcolorSensor.color(
) #Suchen nach schwarzer Linie rechts vom Roboter
if (color2 == Color.BLACK): #Wenn eine schwarze Linie zu sehen ist,
brick.display.text("Drehe n. Rechts",
(0, 50)) #Wird dieseer Text angezeigt
rightMotor.run(200) #Der rechte Motor fährt
leftMotor.run(100) #Der linke Motor fährt langsamer als der Rechte
d = ultrasonicSensor.distance(
) #Die Distanz zwischen Roboter und Würfel wird gemessen
gyro.reset_angle(0) #Der Winkelwert des gyrosensors wird auf 0 gesetzt
#Der Würfel wird gegriffen
rot = abs(
gyro.angle()) #Der totale Wert der Ausgabe des Gyrosensors wird gemessen
while rot < 190: #Solange die Rotation unter 190 ist,
leftMotor.run(
800) #Dreht sich der Roboter, indem nur der linke Motor fährt
rot = abs(gyro.angle()
) #Der totale Wert der Ausgabe des Gyrosensors wird gemessen
leftMotor.run(200) #Ab hier wird Zeile 20-35 wiederholt
rightMotor.run(200)
while True:
class RobotGyro:
"""
This is a facade class for interacting either with
the ev3brick Gyro, or our simulated Gyro,
which needs details about the robot to figure out
what angle it should be reading.
"""
def __init__(self,
port,
left_motor,
right_motor,
wheel_radius,
axis_radius,
debug=False):
self.port = port
self.left_motor = left_motor
self.right_motor = right_motor
self.wheel_radius = wheel_radius
self.axis_radius = axis_radius
self.debug = debug
# either connect to real hardware, or our dummy sim class
self.gyro = GyroSensor(self.port)
self.degrees = int(0)
def reset_gyro_angle(self):
self.gyro.reset_angle(0)
self.degrees = 0
def angle(self):
if not SIM:
# not a simulation, so return what the hardware
# is reading
self.degrees = self.gyro.angle()
return self.degrees
# in simulation mode, the angle returned by the gyro sensor
# depends on how our sim motors are moving.
# If they have moved identically all the time,
# our sim angle will always be zero
left_angle = self.left_motor.angle()
right_angle = self.right_motor.angle()
tol = 1.
angle_diff = abs(left_angle - right_angle)
if angle_diff < tol:
return int(0)
# TBF: how to compute this angle?
# print("Cannot compute angle", angle_diff)
# return None
# we'll only compute spinnging angles: angles from
# two motors are close to eachother but in opposite directions
return self.compute_robot_spin_degrees(left_angle, right_angle)
def compute_robot_spin_degrees(self, left_angle, right_angle):
"Uses configuration of robot and math to figure out gyro value"
if self.debug:
print("compute spin degrees", left_angle, right_angle)
# assert ((right_angle >= 0 and left_angle <= 0) or (right_angle <= 0 and left_angle >= 0))
spin_left = right_angle > 0
positive_wheel_degrees = right_angle if right_angle > 0 else left_angle
# how far has the wheel going forward moved?
# C = 2*pi*r
# distance wheel traveled = (radians traveled)*wheel_radius
# wheel_distance = self.deg2rad(positive_wheel_degrees) * self.wheel_radius
# print("Forward wheel has moved (inches)", wheel_distance)
# so, one wheel has gone part of circle the above distance;
# what angle for this zero-radius turn has it gone?
# C = 2*pi*r
# part of circle = (radians traveled on circle) * (radius of circle)
# robot_spin_degrees = wheel_distance / self.axis_radius
# print("my robot_spin_degrees: ", robot_spin_degrees)
# I keep screwing this up, so here's it explained, and simplified:
# https://www.google.com/url?sa=t&rct=j&q=&esrc=s&source=web&cd=12&ved=2ahUKEwi8y-K0pM_oAhVAgXIEHdapC8cQFjALegQIAxAB&url=https%3A%2F%2Fsheldenrobotics.com%2Ftutorials%2FDetailed_Turning_Tutorial.pdf&usg=AOvVaw3FCQ_gyV_xrhMu2iZsDdBl
# Number of Degrees to Program = (Cturn) / (Cwheel) x (theta)
# positive_wheel_degrees = (self.axis_radius/self.wheel_radius) * robot_spin_degree
robot_spin_degrees = positive_wheel_degrees * (self.wheel_radius /
self.axis_radius)
return robot_spin_degrees if not spin_left else -robot_spin_degrees
def deg2rad(self, degrees):
return degrees * (math.pi / 180.)
def rad2deg(self, radians):
return radians * (180. / math.pi)
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)
wait(500)
gsnow = gyro_angle_1 = gyroSensor.angle()
print('GS Calibration finish ' + str(gsnow))
#calibrategs(gyroSensor)
while True:
ev3.screen.clear()
#share model, basketvball
if Button.UP in ev3.buttons.pressed():
#ev3.speaker.say('Run 1')
#Run 1
accangle = 0
gyroSensor.reset_angle(0)
straight(
200, 200
) # moving to follow line that leads to line that leads to share model
line_follow(
rightcolor, 200, 1.8, 0.08, 0.1, 0.01,
200) # follow line that leads to line that leads to share model
straight(100, 200) # drive forwards a bit
turntoangle(60, True) # turn to follow line that leads to share model
line_follow(rightcolor, 200, 1.5, 0.08, 0.1, 0.01, 330)
straight(50, 130, sampleFactor=3) # drops red cube
straight(30, -200)
turntoangle(31.5, True)
mediummotor.run_angle(800, 1190) #arm goes down
oldstraight(90, 150) #move into basketball
wait(500)
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)
# Write your program here
motorA = Motor(Port.A)
motorC = Motor(Port.C)
robot = DriveBase(motorA, motorC, 56, 114)
gs = GyroSensor(Port.S4)
x = 0
while x < 2:
print("Side 1")
robot.drive_time(500, 0, 2000)
motorA.run(250)
motorC.run(-250)
gs.reset_angle(0)
while gs.angle() >= -75:
wait(50)
print("Gyro Angle :", gs.angle())
if gs.angle() <= -75:
robot.stop(Stop.BRAKE)
robot.stop(Stop.BRAKE)
robot.drive_time(500, 0, 3500)
print("side 2")
motorA.run(250)
motorC.run(-250)
stop1()
elif Button.UP in brick.buttons():
while not Button.CENTER in brick.buttons():
reset_all()
while motor_avarge()<=780:
PID(150,0,8,1,1)
stop1()
robot.stop()
left_M.reset_angle(0)
right_M.reset_angle(0)
while motor_avarge()<=470:
followline(100,1.2,1,1)
stop1()
robot.stop()
time.sleep(0.1)
first_gyro.reset_angle(0)
second_gyro.reset_angle(0)
while first_gyro.angle()<=25:
robot.drive(10,-100)
stop1()
robot.stop()
reset_all()
while motor_avarge()<=1000:
PID(160,0,8,1,1)
stop1()
robot.stop()
time.sleep(0.2)
left_M.reset_angle(0)
right_M.reset_angle(0)
if second_cls.reflection()>=65:
while motor_avarge()<=500:
# Click "Open user guide" on the EV3 extension tab for more information.
# Create your objects here.
ev3 = EV3Brick()
# Write your program here.
ev3.speaker.beep()
left_up_motor = Motor(Port.A, positive_direction=Direction.COUNTERCLOCKWISE)
right_up_motor = Motor(Port.B, positive_direction=Direction.COUNTERCLOCKWISE)
left_down_motor = Motor(Port.C, positive_direction=Direction.CLOCKWISE)
right_down_motor = Motor(Port.D, positive_direction=Direction.CLOCKWISE)
Gyro = GyroSensor(Port.S1, positive_direction=Direction.CLOCKWISE)
Gyro.reset_angle(0)
for i in range(3):
Gyro_angle = Gyro.angle()
#Gyro_speed = Gyro.speed()
print('Gyro angle: ', Gyro_angle)
#print('Gyro speed: ', Gyro_speed)
left_up_motor.run(300)
right_up_motor.run(300)
left_down_motor.run(300)
right_down_motor.run(300)
time.sleep(1)
for i in range(5):
Gyro_angle = Gyro.angle()
#Gyro_speed = Gyro.speed()
mB.set_run_settings(200, 250) # max speed, max accel mC.set_run_settings(200, 250) # max speed, max accel gy1 = GyroSensor(Port.S2) gy2 = GyroSensor(Port.S3) gy3 = GyroSensor(Port.S4) gy1.mode='GYRO-RATE' # deliver turn-rate (must integrate for angle) gy2.mode='GYRO-RATE' # deliver turn-rate (must integrate for angle) gy3.mode='GYRO-RATE' # deliver turn-rate (must integrate for angle) sleep(0.5) gy1.mode='GYRO-ANG' # changing modes causes recalibration gy2.mode='GYRO-ANG' # changing modes causes recalibration gy3.mode='GYRO-ANG' # changing modes causes recalibration sleep(3) gy1.reset_angle(0) gy2.reset_angle(0) gy3.reset_angle(0) mB.reset_angle(0) mC.reset_angle(0) runB = False # whether Motor B should be running right now runC = False t = Thread(target=printAngle) tstart = watch.time() / 1000 t.start() # start angle monitor routine brick.sound.beep(400, 10) # initialization-complete sound # ==========================================
#!/usr/bin/env pybricks-micropython
from pybricks import ev3brick as brick
from pybricks.ev3devices import Motor, GyroSensor
from pybricks.parameters import Port, Stop, Direction, Button
from pybricks.tools import print, wait, StopWatch
from pybricks.robotics import DriveBase
import time
left = Motor(Port.B)
right = Motor(Port.C)
gyro = GyroSensor(Port.S1)
gyro.reset_angle(0)
data = open("m_angle.data", "w")
def rot(spd=40, ang=0):
while(True):
if(gyro.angle() > ang):
right.run(spd)
left.run(spd * -1)
if(gyro.angle() < ang):
left.run(spd)
right.run(spd * -1)
if(gyro.angle() == ang):
left.stop(Stop.HOLD)
right.stop(Stop.HOLD)
time.sleep(0.5)
if(gyro.angle() == ang):
data.write(str(gyro.angle()) + "\n")
gyro.reset_angle(0)
break
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
