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 turn(angle, max_speed, gyro_port, drive_base): gyro = GyroSensor(gyro_port, Direction.COUNTERCLOCKWISE) ev3 = EV3Brick() ev3.screen.clear() initial_error = gyro.angle() - angle while abs(gyro.angle() - angle) > 2: error = gyro.angle() - angle speed = max_speed * (error / initial_error) ev3.screen.print(gyro.angle(), error, speed) drive_base.drive(0, speed) wait(10)
def fall_check(self): fall_gyro = GyroSensor(Port.S4, Direction.CLOCKWISE) while True: real_angle = fall_gyro.angle() if real_angle > 45 or real_angle < -45: self.brick.speaker.beep() else: wait(30) print(real_angle)
def run(): motor_b = Motor(Port.B) motor_c = Motor(Port.C) Gyro = GyroSensor(Port.S1) while Gyro.angle() <= 65: motor_c.run(900) motor_b.run(-900) motor_c.stop(Stop.BRAKE) motor_b.stop(Stop.BRAKE)
def main(): # sound test log.info('test beep') brick.sound.beep() # color sensor test sensor_color = ColorSensor(ROBOT['sensor_color_port']) log.debug('color sensor: color=%s' % sensor_color.color()) # gyro sensor test sensor_gyro = GyroSensor(ROBOT['sensor_gyro_port']) log.debug('gyro sensor: speed=%d, angle=%d' % (sensor_gyro.speed(), sensor_gyro.angle()))
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 test_gyro_drift(port=Port.S4): gyro = GyroSensor(port) printMsg("Checking for drift.") numTests = 5 drifting = False for i in range(numTests): angle = gyro.angle() printMsg("Test %i/%i = %d" % (i + 1, numTests, angle)) if angle != 0: drifting = True break # no need to keep testing wait(1000) return drifting
class Greyden_Skills: def __init__(self, robot): self.robot = robot try: self.gyro = GyroSensor(Port.S2, Direction.CLOCKWISE) except: self.gyro = None print("No Gyro Sensor plugged into Port 2. Cannot run Greyden's Skills. :-(") print() def gyro_angle(self): if self.gyro is None: return float("nan") else: return self.gyro.angle() def tell_me_about_your_skills(self): print("SKILLS - I can read the Gyro Sensor!") print("SKILLS - I am able to show you the Gyro angle, see",self.gyro_angle()) print()
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)
# need to wait until angle set to zero, then recalibrated # have not tested this... while abs(gyro.angle()) != 0 gyro.reset_angle(0) ev3.speaker.beep() ev3.speaker.beep() ################################################################################# ev3.screen.draw_text(50, 60, "Pigeons!") recalibrateGyro() while robot.distance() < 500: robot.drive(1000, 0) robot.reset() gyro.reset_angle(0) angle = gyro.angle() print ( "gyro1 " + str(angle) ) while abs(angle) < 160: print ( "gyro2 " + str(angle) ) print ( "drive " + str(robot.angle() ) ) robot.drive(10, -180) angle = gyro.angle() robot.reset() while robot.distance() < 500: robot.drive(1000, 0)
#!/usr/bin/env pybricks-micropython from pybricks import ev3brick as brick from pybricks.parameters import (Port, Button) from pybricks.tools import (wait, print) from pybricks.ev3devices import GyroSensor #Gyro Sensor auf Port 4 gyro = GyroSensor(Port.S4) #Aktuelle Position als Nullposition festlegen gyro.reset_angle(0) #aktuellen Winkel lesen rot = gyro.angle() #aktuelle Winkelgeschwindigkeit lesen rot_speed = gyro.speed() while not Button.DOWN in brick.buttons(): print(gyro.angle())
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 robot.stop() speed=50 while (i<2 ): fruit = gyro_sensor.angle() fruit = fruit * (-1) robot.drive(speed,fruit) wait(3) robot.drive((speed* (-1)),fruit) i = i + 1
#!/usr/bin/env pybricks-micropython from pybricks import ev3brick as brick from pybricks.ev3devices import (Motor, TouchSensor, ColorSensor, InfraredSensor, UltrasonicSensor, GyroSensor) from pybricks.parameters import (Port, Stop, Direction, Button, Color, SoundFile, ImageFile, Align) from pybricks.tools import print, wait, StopWatch from pybricks.robotics import DriveBase # Write your program here gyro = GyroSensor(Port.S2) sensor = UltrasonicSensor(Port.S1) gyro.reset_angle(0) left = Motor(Port.B) right = Motor(Port.C) robot = DriveBase(left, right, 56, 114) print(gyro.angle()) robot.drive(100, 180) wait(500) print(gyro.angle())
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() print('Gyro angle: ', Gyro_angle) #print('Gyro speed: ', Gyro_speed) left_up_motor.run(300)
while not Button.CENTER in brick.buttons(): reset_all() while motor_avarge()<=780: PID(150,0,8,1,1) stop1() robot.stop() left_M.reset_angle(0) right_M.reset_angle(0) while motor_avarge()<=470: followline(100,1.2,1,1) stop1() robot.stop() time.sleep(0.1) first_gyro.reset_angle(0) second_gyro.reset_angle(0) while first_gyro.angle()<=25: robot.drive(10,-100) stop1() robot.stop() reset_all() while motor_avarge()<=1000: PID(160,0,8,1,1) stop1() robot.stop() time.sleep(0.2) left_M.reset_angle(0) right_M.reset_angle(0) if second_cls.reflection()>=65: while motor_avarge()<=500: followline(100,0.6,1,1) stop1()
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 if rot>=85: #Wenn die Rotation größer oder gleich 85 ist,
motorA = Motor(Port.A) motorD = Motor(Port.D) robot = DriveBase(motorA, motorD, 56, 114) gs = GyroSensor(Port.S2) x = 0 while x < 4: #Drive the robot for 500 mm/s, 0 turn rate and for 2 seconds robot.drive_time(500, 0, 2000) robot.stop(stop_type=Stop.BRAKE) wait(500) print("Robot moved forward. Square side count = ", x + 1) #Reset Gyro Angle before start to turn gs.reset_angle(0) print("Gyro Angle :", gs.angle()) #Make the robot to turn motorA.run(-250) motorD.run(250) while gs.angle() <= 75: wait(50) print("Gyro Angle :", gs.angle()) robot.stop(Stop.BRAKE) x = x + 1
# Initialize gyro. gyro = GyroSensor(Port.S3) gyro.reset_angle(0) # Initialize the drive base. robot = DriveBase(left_motor, right_motor, wheel_diameter=54.6, axle_track=104.1) ################################################################################# ev3.screen.draw_text(50, 60, "Pigeons!") x = 0 while x < 16: # straight (if run motor faster, must adjust distance) robot.reset() while robot.distance() < 250: robot.drive(200, 0) print("gyro") # turn (if run motor faster, must adjust angle) gyro.reset_angle(0) while abs(gyro.angle()) < 66: print("angle: " + str(abs(gyro.angle()))) robot.drive(200, 150) x = x + 1 robot.stop()
################################################################################# # straight (if run motor faster, must adjust distance) # turn (if run motor faster, must adjust angle) ev3.screen.draw_text(50, 60, "Pigeons!") ## 1. ### straight robot.reset() while robot.distance() < 250: robot.drive(200 ,0) ### turn gyro.reset_angle(0) while abs(gyro.angle()) < 66: robot.drive(200 ,150) ## 2. ### straight robot.reset() while robot.distance() < 250: robot.drive(200 ,0) ### turn gyro.reset_angle(0) while abs(gyro.angle()) < 66: robot.drive(200 ,150) ## 3. ### straight
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
vectory = [] ggr = [] results = [] for i in range(0, nb_mesures): result = sens.distance() results.append(int(result)) a = result * cos((2 * pi * i) / nb_mesures) b = result * sin((2 * pi * i) / nb_mesures) vectorx.append(int(a)) vectory.append(int(b)) #LeftMotor.run_angle(motor_speed, int(motor_rotation/nb_mesures), wait=False) # On fait tourner les moteurs. A changer en fonction de la structure du véhicule #RightMotor.run_angle(-motor_speed, int(motor_rotation/nb_mesures), wait=True) LeftMotor.run(motor_speed) #, wait=False) RightMotor.run(-motor_speed) #, wait=False) while (gyro.angle() < (360 / nb_mesures)): res = sens.distance() LeftMotor.stop() ggr.append(gyro.angle()) RightMotor.stop() gyro.reset_angle(0) LeftMotor.run(motor_speed) #, wait=False) RightMotor.run(-motor_speed) #, wait=False) while (gyro.angle() < (360 * (results.index(max(results))) / nb_mesures)): res = sens.distance() LeftMotor.run(-500) RightMotor.run(-500) distance1 = sens.distance() while (sens.distance() > 1000):
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) gs.reset_angle(0)
c = range(41) for i in c: first_val = r_color.reflection() - 25 if first_val > 0: l_motor.run_time(200, 4 * (first_val + 20), wait=False) r_motor.run_time(200, 1 * (first_val + 20)) elif first_val < 0: r_motor.run_time(200, 5 * abs(first_val - 50), wait=False) l_motor.run_time(200, 2 * abs(first_val - 50)) else: r_motor.run_time(200, 120, wait=False) l_motor.run_time(200, 120) gyro.reset_angle(0) while True: if gyro.angle() >= 67: break else: ev3.screen.print(gyro.angle()) r_motor.run_time(100, 100, wait=False) c = range(39) for i in c: first_val = r_color.reflection() - 25 if first_val > 0: l_motor.run_time(200, 4 * (first_val + 20), wait=False) r_motor.run_time(200, 1 * (first_val + 20)) elif first_val < 0: r_motor.run_time(200, 5 * abs(first_val - 50), wait=False) l_motor.run_time(200, 2 * abs(first_val - 50)) else:
motor = Motor(Port.D, Direction.CLOCKWISE) starttime = utime.ticks_ms() b = 0 while (not touch.pressed()) and (b < 3000): motor.run(360) currenttime = utime.ticks_ms() b = currenttime - starttime if motor.stalled(): break motor = Motor(Port.D, Direction.COUNTERCLOCKWISE) turn(1, (gg.angle() - anglecounter * 10)) def turn(dir, ang): if dir == 1: frontmotor.run_angle(100, ang) else: frontmotor.run_angle(100, -ang) while True: print("angle", gg.angle()) motor.run(360) if dist.distance() < 100 or motor.stalled(): motor.stop() reverse() anglecounter += 1 print(gg.angle() - anglecounter * 10) utime.sleep(10)
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 # Config ev3 = EV3Brick() left_motor = Motor(Port.B) right_motor = Motor(Port.C) right_ultra = UltrasonicSensor(Port.S4) front_ultra = UltrasonicSensor(Port.S3) gyro = GyroSensor(Port.S2) data = [] gyro.reset_angle(0) count = 0 while gyro.angle() < 320: left_motor.run(200) right_motor.run(-200) print(front_ultra.distance()) data.append(front_ultra.distance()) print(data)
playerPositionX = 0 if enemy1PositionY > screenHeight: enemy1PositionY = -enemyHeight enemy1PositionX = randint(0, screenWidth - 60) if enemy2PositionY > screenHeight: enemy2PositionY = -enemyHeight enemy2PositionX = randint(0, screenWidth - 90) enemy1PositionY += 12 enemy2PositionY += 12 #if playerPositionX >= enemy1PositionX and playerPositionX <= (enemy1PositionX + enemyWidth) and playerPositionY >= enemy1PositionY and playerPositionY <= (enemy1PositionY + enemyHeight): # break # if (playerPositionX + playerWidth) >= enemy1PositionX and (playerPositionX + playerWidth) <= (enemy1PositionX + enemyWidth) and playerPositionY >= enemy1PositionY and playerPositionY <= (enemy1PositionY + enemyHeight): # break if touchR.pressed(): break # motorL.run_angle(100, gyro.angle() * 100, Stop.COAST, False) #motorR.run_angle(100, -(gyro.angle() * 100), Stop.COAST, False) playerPositionX -= gyro.angle() wait(60) brick.display.clear() brick.sound.file(SoundFile.GAME_OVER) brick.display.image('GameOver.png') wait(3000)
medium_left.run_time(-200, 4000) robot.straight(110) medium_left.run_time(150, 6500) #back after both tire flip robot.turn(20) robot.straight(-550) robot.straight(150) robot.turn(-125) robot.straight(300) robot.stop() StopAtWhiteLineRight() gs.reset_angle(0) #parrallel park while (gs.angle() > -35): right_motor.run(300) left_motor.run(215) print(gs.angle()) robot.stop() while (gs.angle() < 0): right_motor.run(215) left_motor.run(300) print(gs.angle()) robot.stop() #pushes step counter robot.straight(-500)
steps = 0 # If the Center Button is pressed, break out of the loop. elif Button.CENTER in ev3.buttons.pressed(): break # This loop climbs the stairs for the amount of steps specified in the # steps variable. It repeats until the steps variable is 0. while steps > 0: # Run the front and rear motors so the robot moves forward. front_motor.dc(100) rear_motor.dc(90) # Keep moving until the robot is at an angle of at least 10 degrees. while gyro_sensor.angle() < 10: wait(10) # Run the lift motor to move the rear structure up, while # simultaneously running the front and rear motors. lift_motor.dc(90) front_motor.dc(30) rear_motor.dc(15) # Keep moving the rear structure up until the Touch Sensor is # pressed, or the robot is at an angle of less than -3 degrees. while not touch_sensor.pressed(): if gyro_sensor.angle() < -3: break wait(10) lift_motor.hold()
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)