class Parts(): def __init__(self): self.ev3 = EV3Brick() #self.motorleft = Motor(Port.D) self.motorright = Motor(Port.A) self.right = ColorSensor(Port.S3) self.left = ColorSensor(Port.S2) #self.gyro = GyroSensor(Port.S4) self.leftambient = 0 self.rightambient = 0 self.delta = 0 #self.arrayturnleft = [0, 0, 0, 0, 0] #self.arrayturnright = [0, 0, 0, 0, 0] #self.savefile = open("testcheck.txt", "a") def check(self): self.leftambient = self.left.ambient() self.rightambient = self.right.ambient() self.delta = self.leftambient - self.rightambient #return [self.leftambient, self.rightambient] return self.delta #print("LEFT: %d | RIGHT: %d" % (self.leftambient, self.rightambient)) def checkGyro(self): print(self.gyro) def whileCheck(self): #for x in range(5): # self.arrayturnleft[x], self.arrayturnleft[x] = self.check() # time.sleep(0.1) #self.delta = (sum(self.arrayturnleft)/len(self.arrayturnleft) - sum(self.arrayturnright)/len(self.arrayturnright)) #print(self.delta) if (self.check() > 0.5): #self.delta if (self.motorright.angle() > 45): print("OVERRIDE") else: self.motorright.run_angle(10, -10) #10 left print("TURNED THIS WAY") elif (self.check() < -0.5): #self.delta if (self.motorright.angle() < -45): print("OVERRIDE") else: self.motorright.run_angle(10, 10) #-10 right print("TURNED THAT WAY") #brickthing.savefile.write("LEFT: %d | RIGHT: %d" % (brickthing.leftambient, brickthing.rightambient))''' def rapidcheck(self): while brickthing.check() > 1 or brickthing.check( ) < -1: #while True: | self.delta, self.delta brickthing.whileCheck() print(brickthing.check()) print("BREAK") time.sleep(5)
class SmallMotor: def __init__(self, ev3, port): self.ev3 = ev3 self.motor = Motor(port, Direction.COUNTERCLOCKWISE) self.motor.reset_angle(0) def reset(self): self.motor.run_until_stalled(100) self.motor.run_angle(800, -300) self.motor.reset_angle(0) def moveTo(self, angle, speed = 800, wait = False): print(self.motor.angle()) self.motor.run_target(speed, angle, Stop.HOLD, wait) print(self.motor.angle()) def move(self, speed = 20): self.motor.run(speed) def brake(self): self.motor.brake()
class El3ctricGuitar: NOTES = [1318, 1174, 987, 880, 783, 659, 587, 493, 440, 392, 329, 293] N_NOTES = len(NOTES) def __init__(self, lever_motor_port: Port = Port.D, touch_sensor_port: Port = Port.S1, ir_sensor_port: Port = Port.S4): self.ev3_brick = EV3Brick() self.lever_motor = Motor(port=lever_motor_port, positive_direction=Direction.CLOCKWISE) self.touch_sensor = TouchSensor(port=touch_sensor_port) self.ir_sensor = InfraredSensor(port=ir_sensor_port) def start_up(self): self.ev3_brick.screen.load_image(ImageFile.EV3) self.ev3_brick.light.on(color=Color.ORANGE) self.lever_motor.run_time(speed=50, time=1000, then=Stop.COAST, wait=True) self.lever_motor.run_angle(speed=50, rotation_angle=-30, then=Stop.BRAKE, wait=True) wait(100) self.lever_motor.reset_angle(angle=0) def play_note(self): if not self.touch_sensor.pressed(): raw = sum(self.ir_sensor.distance() for _ in range(4)) / 4 self.ev3_brick.speaker.beep( frequency=self.NOTES[min(int(raw / 5), self.N_NOTES - 1)] - 11 * self.lever_motor.angle(), duration=100) wait(1)
def kalibriere(self): headmotor = Motor(Port.A, Direction.COUNTERCLOCKWISE) farbsensor = ColorSensor(Port.S3) headmotor.run_until_stalled(speed=10, duty_limit=50) debug('winkel=' + str(headmotor.angle())) headmotor.run_target(speed=10, target_angle=-120, wait=False) while (farbsensor.reflection() < 10): # & (headmotor.speed() != 0): debug('farbwert=' + str(farbsensor.reflection())) time.sleep(0.1) headmotor.stop() headmotor.run_angle(speed=10, rotation_angle=15) debug(str(farbsensor.reflection())) # winkel auf 0 headmotor.reset_angle(0) self.angle_ist = 0 self._schreibe_winkel()
class Dinor3x(EV3Brick): """ Challenges: - Can you make DINOR3X remote controlled with the IR-Beacon? - Can you attach a colorsensor to DINOR3X, and make it behave differently depending on which color is in front of the sensor (red = walk fast, white = walk slow, etc.)? """ def __init__( self, left_motor_port: Port = Port.B, right_motor_port: Port = Port.C, jaw_motor_port: Port = Port.A, touch_sensor_port: Port = Port.S1, ir_sensor_port: Port = Port.S4, ir_beacon_channel: int = 1): self.left_motor = Motor(port=left_motor_port, positive_direction=Direction.CLOCKWISE) self.right_motor = Motor(port=right_motor_port, positive_direction=Direction.CLOCKWISE) self.jaw_motor = Motor(port=jaw_motor_port, positive_direction=Direction.CLOCKWISE) self.touch_sensor = TouchSensor(port=touch_sensor_port) self.ir_sensor = InfraredSensor(port=ir_sensor_port) self.ir_beacon_channel = ir_beacon_channel def calibrate_legs(self): self.left_motor.run(speed=100) self.right_motor.run(speed=200) while self.touch_sensor.pressed(): pass self.left_motor.hold() self.right_motor.hold() self.left_motor.run(speed=400) while not self.touch_sensor.pressed(): pass self.left_motor.hold() self.left_motor.run_angle( rotation_angle=-0.2 * 360, speed=500, then=Stop.HOLD, wait=True) self.right_motor.run(speed=400) while not self.touch_sensor.pressed(): pass self.right_motor.hold() self.right_motor.run_angle( rotation_angle=-0.2 * 360, speed=500, then=Stop.HOLD, wait=True) self.left_motor.reset_angle(angle=0) self.right_motor.reset_angle(angle=0) def roar(self): self.speaker.play_file(file=SoundFile.T_REX_ROAR) self.jaw_motor.run_angle( speed=400, rotation_angle=-60, then=Stop.HOLD, wait=True) # FIXME: jaw doesn't close for i in range(12): self.jaw_motor.run_time( speed=-400, time=0.05 * 1000, then=Stop.HOLD, wait=True) self.jaw_motor.run_time( speed=400, time=0.05 * 1000, then=Stop.HOLD, wait=True) self.jaw_motor.run(speed=200) sleep(0.5) def close_mouth(self): self.jaw_motor.run(speed=200) sleep(1) self.jaw_motor.stop() def walk_until_blocked(self): self.left_motor.run(speed=-400) self.right_motor.run(speed=-400) while self.ir_sensor.distance() >= 25: pass self.left_motor.stop() self.right_motor.stop() def run_away(self): self.left_motor.run_angle( speed=750, rotation_angle=3 * 360, then=Stop.BRAKE, wait=False) self.right_motor.run_angle( speed=750, rotation_angle=3 * 360, then=Stop.BRAKE, wait=True) def jump(self): """ Dinor3x Mission 02 Challenge: make it jump """ ... # TRANSLATED FROM EV3-G MY BLOCKS # ------------------------------- def leg_adjust( self, cyclic_degrees: float, speed: float = 1000, leg_offset_percent: float = 0, mirrored_adjust: bool = False, brake: bool = True): ... def leg_to_pos( self, speed: float = 1000, left_position: float = 0, right_position: float = 0): self.left_motor.brake() self.right_motor.brake() self.left_motor.run_angle( speed=speed, rotation_angle=left_position - cyclic_position_offset( rotation_sensor=self.left_motor.angle(), cyclic_degrees=360), then=Stop.BRAKE, wait=True) self.right_motor.run_angle( speed=speed, rotation_angle=right_position - cyclic_position_offset( rotation_sensor=self.right_motor.angle(), cyclic_degrees=360), then=Stop.BRAKE, wait=True) def turn(self, speed: float = 1000, n_steps: int = 1): ... def walk(self, speed: float = 1000): ... def walk_steps(self, speed: float = 1000, n_steps: int = 1): ...
def takeSecond(elem): return elem[0] def KNN(categories, distance, x, k): dist = [] for (i, val) in enumerate(distance): dist.append((abs(val-x),categories[i])) dist.sort(key=takeSecond) sum = 0 for i in range(3): sum += dist[i][1] return(sum/3) #this is actual k means # return 0 if sum < (k-sum) else 1 # return 0 if mostly 0s categories = [75,75,75,80,80,80,85,85,85,90,90,90,95,95,95] distance = [32,49,68,139,157,145,199,209,197,243,259,250,298,290,302] while True: speed = KNN(categories,distance,ultra.distance() + 100 ,3) if touch.pressed(): for i in range(1000): if abs(throw.angle())< abs(115): throw.dc(speed) else: break wait(0.1) print('done') throw.run_angle(2,2,stop_type=Stop.COAST, wait=True)
if sensorDirection == 1: DrawText("CANNONS UP!", 1) sensorMotorLeft.run(-sensorMoveSpeed) sensorMotorRight.run(-sensorMoveSpeed) elif sensorDirection == 2: DrawText("GET DOWN MR PRESIDENT!", 1) sensorMotorLeft.run(sensorMoveSpeed) sensorMotorRight.run(sensorMoveSpeed) else: sensorMotorLeft.stop() sensorMotorRight.stop() # Robot state hasRecentlyFallenBackIR = False initialSensorAngleLeft = sensorMotorLeft.angle( ) # left sensor motor is used to measure the sensor angle # Main loop loopCount = 0 # we only update some (slow) logic every X logical frames while (True): ev3.screen.clear() DrawText("~~ JL ROBOT " + str(loopCount) + " ~~", 4) # Outputs moveDirection = 0 # 0 = stop, 1 = forward, 2 = backward moveImpulseTime = -1 # milliseconds, -1 = continious sensorDirection = 0 # Read infrared beacon if (enableBeaconMoves and loopCount % 100 == 0): beaconData = ir.beacon(infraredChannel)
if (data): #brick.sound.file(SoundFile.FOUR) tx_str = 'received string:[' + str(data) + ']' +'\n' s.send(bytes(tx_str, 'utf-8')) if 'turn_to_' in str(data): rx_str = str(data) number_str = chr(data[8]) + chr(data[9]) + chr(data[10]) number = int(number_str) tx_str = 'decoded number is:[' + str(number) + ']' +'\n' s.send(bytes(tx_str, 'utf-8')) turn(number) data=None if 'get_turn_angle' in str(data): tx_string = 'turn_motor.angle()=' + str(turn_motor.angle()) + '\n' s.send(bytes(tx_string, 'utf-8')) data = None if 'set_turn_angle_90' in str(data): turn_motor.reset_angle(90) brick.sound.beep() data = None if 'arm_down' in str(data): arm_down() brick.sound.beep() data = None if 'arm_up' in str(data): arm_up()
#!/usr/bin/env pybricks-micropython from pybricks import ev3brick as brick from pybricks.ev3devices import Motor from pybricks.parameters import Port, Button from pybricks.tools import print, wait motor = Motor(Port.B) cycle = 50 while True: bts = brick.buttons() if Button.LEFT in bts: cycle = max(-100, cycle - 10) elif Button.RIGHT in bts: cycle = min(100, cycle + 10) elif Button.CENTER in bts: break motor.dc(cycle) print(cycle, motor.speed(), motor.angle()) wait(100)
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
class MyRobot: def __init__(self, ev3, leftMotorPort, rightMotorPort, leftColorSensorPort, rightColorSensorPort): self.ev3 = ev3 self.leftMotor = Motor(leftMotorPort) self.rightMotor = Motor(rightMotorPort) self.rightMotor.control.limits(800, 500, 100) self.leftMotor.control.limits(800, 500, 100) if (leftColorSensorPort != None): self.leftColorSensor = RGBColor(leftColorSensorPort) print(self.leftColorSensor.getReflection()) if (rightColorSensorPort != None): self.rightColorSensor = RGBColor(rightColorSensorPort) print(self.rightColorSensor.getReflection()) ev3.speaker.set_speech_options('hu', 'f2', 200) def beep(self, hangmagassag=600, duration=100): self.ev3.speaker.beep(frequency=hangmagassag, duration=duration) def forwardAndStop(self, speed, angle): self.leftMotor.run_angle(speed, angle, Stop.HOLD, False) self.rightMotor.run_angle(speed, angle, Stop.HOLD, True) def forward(self, speed, angle): leftAngle = self.leftMotor.angle() rightAngle = self.rightMotor.angle() self.leftMotor.run(speed) self.rightMotor.run(speed) if (speed > 0): while (((self.leftMotor.angle() - leftAngle) + (self.rightMotor.angle() - rightAngle)) / 2 < angle): pass else: while (((leftAngle - self.leftMotor.angle()) + (rightAngle - self.rightMotor.angle())) / 2 < angle): pass def brake(self): self.leftMotor.brake() self.rightMotor.brake() def leftAngle(self): return self.leftMotor.angle() def rightAngle(self): return self.rightMotor.angle() def resetAngle(self): self.leftMotor.reset_angle(0) self.rightMotor.reset_angle(0) def turnLeft(self, speed): self.leftMotor.run_angle(speed, -148, Stop.HOLD, False) self.rightMotor.run_angle(speed, 148, Stop.HOLD, True) def turnRight(self, speed): self.leftMotor.run_angle(speed, 148, Stop.HOLD, False) self.rightMotor.run_angle(speed, -148, Stop.HOLD, True) def turnLeftWithRightMotor(self, speed): self.rightMotor.run_angle(speed, 296, Stop.HOLD, True) def turnLeftWithLeftMotor(self, speed): self.leftMotor.run_angle(speed, -296, Stop.HOLD, True) def turnRightWithRightMotor(self, speed): self.rightMotor.run_angle(speed, -296, Stop.HOLD, True) def turnRightWithLeftMotor(self, speed): self.leftMotor.run_angle(speed, 296, Stop.HOLD, True) def forwardWhile(self, speed, leftMotorConditionFunc, rightMotorConditionFunc): self.leftMotor.run(speed) self.rightMotor.run(speed) while (leftMotorConditionFunc() and rightMotorConditionFunc()): pass if (not leftMotorConditionFunc()): self.leftMotor.brake() while (rightMotorConditionFunc()): pass self.rightMotor.brake() else: self.rightMotor.brake() while (leftMotorConditionFunc()): pass self.leftMotor.brake() def say(self, text): return Thread(target=self.ev3.speaker.say(text)) def alignToWhite(self, speed, whiteThreshold): self.leftMotor.run(speed) self.rightMotor.run(speed) time.sleep(0.1) while (self.leftMotor.speed() != 0 or self.rightMotor.speed() != 0): if (self.leftColorSensor.getReflection() > whiteThreshold): self.leftMotor.brake() if (self.rightColorSensor.getReflection() > whiteThreshold): self.rightMotor.brake() def alignToBlack(self, speed, blackThreshold): self.leftMotor.run(speed) self.rightMotor.run(speed) time.sleep(0.1) while (self.leftMotor.speed() != 0 or self.rightMotor.speed() != 0): if (self.leftColorSensor.getReflection() < blackThreshold): self.leftMotor.brake() if (self.rightColorSensor.getReflection() < blackThreshold): self.rightMotor.brake() def alignToNotWhite(self, speed, whiteThreshold): self.leftMotor.run(speed) self.rightMotor.run(speed) time.sleep(0.1) while (self.leftMotor.speed() != 0 or self.rightMotor.speed() != 0): leftReflection = self.leftColorSensor.getReflection() rightReflection = self.rightColorSensor.getReflection() if (leftReflection < whiteThreshold): self.leftMotor.brake() if (rightReflection < whiteThreshold): self.rightMotor.brake() # print("r = {0}, l = {1}".format(rightReflection, leftReflection)) def measureColorSensors(self): while Button.CENTER not in self.ev3.buttons.pressed(): print('left reflection: {0}, right reflection: {1}'.format( self.leftColorSensor.getReflection(), self.rightColorSensor.getReflection())) time.sleep(0.2)
from pybricks.hubs import EV3Brick from pybricks.tools import wait, StopWatch, DataLog from pybricks.parameters import Color, Port from pybricks.ev3devices import Motor from pybricks.iodevices import AnalogSensor, UARTDevice # Initialize the EV3 ev3 = EV3Brick() ev3.speaker.beep() sense = AnalogSensor(Port.S1, False) sense.voltage() watch = StopWatch() wheel = Motor(Port.A) data = DataLog('output.txt', header='Time,Angle,Voltage') # Turn on a red light ev3.light.on(Color.RED) ev3.speaker.say("About to take data") wheel.run(500) for i in range(10): time = watch.time() angle = wheel.angle() light = sense.voltage() #This seems to give a EIO error sometimes. data.log(time, angle, light) wait(100) # Turn the light off ev3.light.off()
class Charlie(): ''' Charlie is the class responsible for driving, Robot-Movement and general Real-world interaction of the robot with Sensors and motors. Args: config (dict): The parsed config brick (EV3Brick): EV3Brick for getting button input logger (Logger): Logger for logging ''' def __init__(self, config, brick, logger): logger.info(self, 'Starting initialisation of Charlie') self.__config = config self.brick = brick self.logger = logger self.__conf2port = { 1: Port.S1, 2: Port.S2, 3: Port.S3, 4: Port.S4, 'A': Port.A, 'B': Port.B, 'C': Port.C, 'D': Port.D } self.__initSensors() self.__initMotors() self.min_speed = 35 # lage motor 20, medium motor 30 self.__gyro.reset_angle(0) self.__screenRoutine = False self.showDetails() self.logger.info(self, 'Driving for Charlie initialized') ##TODO def __repr__(self): outputString = "(TODO)\n Config: " + self.__config + "\n Brick: " + self.brick + "\n Logger: " + self.logger outputString += "\n--Debug--\n Minimum Speed: " + str( self.min_speed) + "\n " return "TODO" def __str__(self): return "Charlie" def __initSensors(self): '''Sub-method for initializing Sensors.''' self.logger.debug(self, "Starting sensor initialisation...") try: self.__gyro = GyroSensor( self.__conf2port[self.__config['gyroSensorPort']], Direction.CLOCKWISE if not self.__config['gyroInverted'] else Direction.COUNTERCLOCKWISE ) if self.__config['gyroSensorPort'] != 0 else 0 self.logger.debug( self, 'Gyrosensor initialized sucessfully on port %s' % self.__config['gyroSensorPort']) except Exception as exception: self.__gyro = 0 self.logger.error( self, "Failed to initialize the Gyro-Sensor - Are u sure it's connected to Port %s?" % exception, exception) try: self.__rLight = ColorSensor( self.__conf2port[self.__config['rightLightSensorPort']] ) if self.__config['rightLightSensorPort'] != 0 else 0 self.logger.debug( self, 'Colorsensor initialized sucessfully on port %s' % self.__config['rightLightSensorPort']) except Exception as exception: self.logger.error( self, "Failed to initialize the right Color-Sensor - Are u sure it's connected to Port %s?" % exception, exception) try: self.__lLight = ColorSensor( self.__conf2port[self.__config['leftLightSensorPort']] ) if self.__config['leftLightSensorPort'] != 0 else 0 self.logger.debug( self, 'Colorsensor initialized sucessfully on port %s' % self.__config['leftLightSensorPort']) except Exception as exception: self.logger.error( self, "Failed to initialize the left Color-Sensor - Are u sure it's connected to Port %s?" % exception, exception) try: self.__touch = TouchSensor( self.__conf2port[self.__config['backTouchSensor']] ) if self.__config['backTouchSensor'] != 0 else 0 self.logger.debug( self, 'Touchsensor initialized sucessfully on port %s' % self.__config['backTouchSensor']) except Exception as exception: self.logger.error( self, "Failed to initialize the Touch-Sensor - Are u sure it's connected to Port %s?" % exception, exception) self.logger.debug(self, "Sensor initialisation done") def __initMotors(self): '''Sub-method for initializing Motors.''' self.logger.debug(self, "Starting motor initialisation...") if self.__config['robotType'] == 'NORMAL': try: self.__lMotor = Motor( self.__conf2port[self.__config['leftMotorPort']], Direction.CLOCKWISE if (not self.__config['leftMotorInverted']) else Direction.COUNTERCLOCKWISE, gears=self.__config['leftMotorGears']) self.__rMotor = Motor( self.__conf2port[self.__config['rightMotorPort']], Direction.CLOCKWISE if (not self.__config['rightMotorInverted']) else Direction.COUNTERCLOCKWISE, gears=self.__config['rightMotorGears']) except Exception as exception: self.logger.error( self, "Failed to initialize movement motors for robot type NORMAL - Are u sure they\'re all connected?", exception) if self.__config['useGearing']: try: self.__gearingPortMotor = Motor( self.__conf2port[ self.__config['gearingSelectMotorPort']], Direction.CLOCKWISE if (not self.__config['gearingSelectMotorInverted']) else Direction.COUNTERCLOCKWISE, gears=self.__config['gearingSelectMotorGears']) self.__gearingTurnMotor = Motor( self.__conf2port[ self.__config['gearingTurnMotorPort']], Direction.CLOCKWISE if (not self.__config['gearingTurnMotorInverted']) else Direction.COUNTERCLOCKWISE, gears=self.__config['gearingTurnMotorGears']) except Exception as exception: self.logger.error( self, "Failed to initialize action motors for the gearing - Are u sure they\'re all connected?", exception) else: try: self.__aMotor1 = Motor( self.__conf2port[ self.__config['firstActionMotorPort']], Direction.CLOCKWISE if (not self.__config['firstActionMotorInverted']) else Direction.COUNTERCLOCKWISE, gears=self.__config['firstActionMotorGears']) if ( self.__config['firstActionMotorPort'] != 0) else 0 self.__aMotor2 = Motor( self.__conf2port[ self.__config['secondActionMotorPort']], Direction.CLOCKWISE if (not self.__config['secondActionMotorInverted']) else Direction.COUNTERCLOCKWISE, gears=self.__config['secondActionMotorGears']) if ( self.__config['secondActionMotorPort'] != 0) else 0 except Exception as exception: self.logger.error( self, "Failed to initialize action motors - Are u sure they\'re all connected?", exception) else: try: self.__fRMotor = Motor( self.__conf2port[self.__config['frontRightMotorPort']], Direction.CLOCKWISE if (not self.__config['frontRightMotorInverted']) else Direction.COUNTERCLOCKWISE, gears=self.__config['frontRightMotorGears']) if ( self.__config['frontRightMotorPort'] != 0) else 0 self.__bRMotor = Motor( self.__conf2port[self.__config['backRightMotorPort']], Direction.CLOCKWISE if (not self.__config['backRightMotorInverted']) else Direction.COUNTERCLOCKWISE, gears=self.__config['backRightMotorGears']) if ( self.__config['backRightMotorPort'] != 0) else 0 self.__fLMotor = Motor( self.__conf2port[self.__config['frontLeftMotorPort']], Direction.CLOCKWISE if (not self.__config['frontLeftMotorInverted']) else Direction.COUNTERCLOCKWISE, gears=self.__config['frontLeftMotorGears']) if ( self.__config['frontLeftMotorPort'] != 0) else 0 self.__bLMotor = Motor( self.__conf2port[self.__config['backLeftMotorPort']], Direction.CLOCKWISE if (not self.__config['backLeftMotorInverted']) else Direction.COUNTERCLOCKWISE, gears=self.__config['backLeftMotorGears']) if ( self.__config['backLeftMotorPort'] != 0) else 0 except Exception as exception: self.logger.error( self, "Failed to initialize movement motors for robot type %s - Are u sure they\'re all connected? Errored at Port" % self.__config['robotType'], exception) self.logger.debug(self, "Motor initialisation done") self.logger.info(self, 'Charlie initialized') def showDetails(self): ''' Processes sensor data in a separate thread and shows ''' threadLock = _thread.allocate_lock() def __screenPrintRoutine(): while True: if self.__gyro.angle() > 360: ang = self.__gyro.angle() - 360 else: ang = self.__gyro.angle() speedRight = self.__rMotor.speed() if self.__config[ 'robotType'] == 'NORMAL' else self.__fRMotor.speed() speedRight = speedRight / 360 # from deg/s to revs/sec speedRight = speedRight * (self.__config['wheelDiameter'] * math.pi) # from revs/sec to cm/sec speedLeft = self.__lMotor.speed() if self.__config[ 'robotType'] == 'NORMAL' else self.__fLMotor.speed() speedLeft = speedLeft / 360 # from deg/s to revs/sec speedLeft = speedLeft * (self.__config['wheelDiameter'] * math.pi) # from revs/sec to cm/sec #self.brick.screen.set_font(Font(family = 'arial', size = 16)) if self.__screenRoutine: print(self.__gyro.angle()) self.brick.screen.draw_text(5, 10, 'Robot-Angle: %s' % ang, text_color=Color.BLACK, background_color=Color.WHITE) self.brick.screen.draw_text(5, 40, 'Right Motor Speed: %s' % ang, text_color=Color.BLACK, background_color=Color.WHITE) self.brick.screen.draw_text(5, 70, 'Left Motor Speed: %s' % ang, text_color=Color.BLACK, background_color=Color.WHITE) time.sleep(0.1) with threadLock: _thread.start_new_thread(__screenPrintRoutine, ()) def execute(self, params): ''' This function interprets the number codes from the given array and executes the driving methods accordingly Args: params (array): The array of number code arrays to be executed ''' if self.brick.battery.voltage() <= 7600: if (self.__config["ignoreBatteryWarning"] == True): self.logger.warn( "Please charge the battery. Only %sV left. We recommend least 7.6 Volts for accurate and repeatable results. ignoreBatteryWarning IS SET TO True, THIS WILL BE IGNORED!!!" % self.brick.battery.voltage() * 0.001) else: self.logger.warn( "Please charge the battery. Only %sV left. We recommend least 7.6 Volts for accurate and repeatable results." % self.brick.battery.voltage() * 0.001) return if self.__gyro == 0: self.logger.error(self, "Cannot drive without gyro", '') return methods = { 3: self.absTurn, 4: self.turn, 5: self.action, 7: self.straight, 9: self.intervall, 11: self.curve, 12: self.toColor, 15: self.toWall } self.__gyro.reset_angle(0) self.__gyro.reset_angle(0) time.sleep(0.1) self.__screenRoutine = True while params != [] and not any(self.brick.buttons.pressed()): pparams = params.pop(0) mode, arg1, arg2, arg3 = pparams.pop(0), pparams.pop( 0), pparams.pop(0), pparams.pop(0) methods[mode](arg1, arg2, arg3) self.breakMotors() if self.__config['useGearing']: self.__gearingPortMotor.run_target(300, 0, Stop.HOLD, True) # reset gearing time.sleep(0.3) self.__screenRoutine = False def turn(self, speed, deg, port): ''' Used to turn the motor on the spot using either one or both Motors for turning (2 or 4 in case of ALLWHEEL and MECANUM) Args: speed (int): the speed to drive at deg (int): the angle to turn port (int): the motor(s) to turn with ''' startValue = self.__gyro.angle() speed = self.min_speed if speed < self.min_speed else speed # turn only with left motor if port == 2: # right motor off self.__rMotor.dc(0) # turn the angle if deg > 0: while self.__gyro.angle() - startValue < deg: self.turnLeftMotor(speed) # slow down to not overshoot if not self.__gyro.angle() - startValue < deg * 0.6: speed = speed - self._map( deg, 1, 360, 10, 0.1) if speed > self.min_speed else self.min_speed #cancel if button pressed if any(self.brick.buttons.pressed()): return else: while self.__gyro.angle() - startValue > deg: self.turnLeftMotor(-speed) # slow down to not overshoot if not self.__gyro.angle() - startValue > deg * 0.6: speed = speed - self._map( deg, 1, 360, 10, 0.1) if speed > self.min_speed else self.min_speed #cancel if button pressed if any(self.brick.buttons.pressed()): return # turn only with right motor elif port == 3: # left motor off self.__lMotor.dc(0) # turn the angle if deg > 0: while self.__gyro.angle() - startValue < deg: self.turnRightMotor(-speed) # slow down to not overshoot if not self.__gyro.angle() - startValue < deg * 0.6: speed = speed - self._map( deg, 1, 360, 10, 0.1) if speed > self.min_speed else self.min_speed #cancel if button pressed if any(self.brick.buttons.pressed()): return else: while self.__gyro.angle() - startValue > deg: self.turnRightMotor(speed) # slow down to not overshoot if not self.__gyro.angle() - startValue > deg * 0.6: speed = speed - self._map( deg, 1, 360, 10, 0.1 ) if speed > self.min_speed else self.min_speed + 5 #cancel if button pressed if any(self.brick.buttons.pressed()): return # turn with both motors elif port == 23: dualMotorbonus = 19 speed = speed * 2 # turn the angle if deg > 0: while self.__gyro.angle() - startValue < deg: self.turnLeftMotor(speed / 2) self.turnRightMotor(-speed / 2) # slow down to not overshoot if not self.__gyro.angle() - startValue < deg * 0.6: speed = speed - self._map( deg, 1, 360, 10, 0.01 ) if speed - self._map( deg, 1, 360, 10, 0.01 ) > self.min_speed * 2 - dualMotorbonus else self.min_speed * 2 - dualMotorbonus # cancel if button pressed if any(self.brick.buttons.pressed()): return else: while self.__gyro.angle() - startValue > deg: self.turnLeftMotor(-speed / 2) self.turnRightMotor(speed / 2) # slow down to not overshoot if not self.__gyro.angle() - startValue > deg * 0.6: speed = speed - self._map( deg, 1, 360, 10, 0.01 ) if speed - self._map( deg, 1, 360, 10, 0.01 ) > self.min_speed * 2 - dualMotorbonus else self.min_speed * 2 - dualMotorbonus # cancel if button pressed if any(self.brick.buttons.pressed()): return def absTurn(self, speed, deg, port): ''' Used to turn the motor on the spot using either one or both Motors for turning (2 or 4 in case of ALLWHEEL and MECANUM) This method turns in contrast to the normal turn() method to an absolute ange (compared to starting point) Args: speed (int): the speed to drive at deg (int): the angle to turn to port (int): the motor(s) to turn with ''' speed = self.min_speed if speed < self.min_speed else speed # turn only with left motor if port == 2: # right motor off self.__rMotor.dc(0) # turn the angle if deg > 0: while self.__gyro.angle() < deg: self.turnLeftMotor(speed) # slow down to not overshoot if not self.__gyro.angle() < deg * 0.6: speed = speed - self._map( deg, 1, 360, 10, 0.1) if speed > self.min_speed else self.min_speed #cancel if button pressed if any(self.brick.buttons.pressed()): return else: while self.__gyro.angle() > deg: self.turnLeftMotor(-speed) # slow down to not overshoot if not self.__gyro.angle() > deg * 0.6: speed = speed - self._map( deg, 1, 360, 10, 0.1) if speed > self.min_speed else self.min_speed #cancel if button pressed if any(self.brick.buttons.pressed()): return # turn only with right motor elif port == 3: # left motor off self.__lMotor.dc(0) # turn the angle if deg > 0: while self.__gyro.angle() < deg: self.turnRightMotor(-speed) # slow down to not overshoot if not self.__gyro.angle() < deg * 0.6: speed = speed - self._map( deg, 1, 360, 10, 0.1) if speed > self.min_speed else self.min_speed #cancel if button pressed if any(self.brick.buttons.pressed()): return else: while self.__gyro.angle() > deg: self.turnRightMotor(speed) # slow down to not overshoot if not self.__gyro.angle() > deg * 0.6: speed = speed - self._map( deg, 1, 360, 10, 0.1 ) if speed > self.min_speed else self.min_speed + 5 #cancel if button pressed if any(self.brick.buttons.pressed()): return # turn with both motors elif port == 23: dualMotorbonus = 19 speed = speed * 2 # turn the angle if deg > 0: while self.__gyro.angle() < deg: self.turnLeftMotor(speed / 2) self.turnRightMotor(-speed / 2) # slow down to not overshoot if not self.__gyro.angle() < deg * 0.6: speed = speed - self._map( deg, 1, 360, 10, 0.01 ) if speed - self._map( deg, 1, 360, 10, 0.01 ) > self.min_speed * 2 - dualMotorbonus else self.min_speed * 2 - dualMotorbonus # cancel if button pressed if any(self.brick.buttons.pressed()): return else: while self.__gyro.angle() > deg: self.turnLeftMotor(-speed / 2) self.turnRightMotor(speed / 2) # slow down to not overshoot if not self.__gyro.angle() > deg * 0.6: speed = speed - self._map( deg, 1, 360, 10, 0.01 ) if speed - self._map( deg, 1, 360, 10, 0.01 ) > self.min_speed * 2 - dualMotorbonus else self.min_speed * 2 - dualMotorbonus # cancel if button pressed if any(self.brick.buttons.pressed()): return def straight(self, speed, dist, ang): ''' Drives the Robot in a straight line. Also it self-corrects while driving with the help of a gyro-sensor. This is used to make the Robot more accurate Args: speed (int): the speed to drive at dist (int): the distance in cm to drive ''' if self.__config['robotType'] != 'MECANUM': correctionStrength = 2.5 # how strongly the self will correct. 2 = default, 0 = nothing startValue = self.__gyro.angle() # convert the input (cm) to revs revs = dist / (self.__config['wheelDiameter'] * math.pi) motor = self.__rMotor if self.__config[ 'robotType'] == 'NORMAL' else self.__fRMotor # drive motor.reset_angle(0) if revs > 0: while revs > (motor.angle() / 360): # if not driving staright correct it if self.__gyro.angle() - startValue > 0: lSpeed = speed - abs(self.__gyro.angle() - startValue) * correctionStrength rSpeed = speed elif self.__gyro.angle() - startValue < 0: rSpeed = speed - abs(self.__gyro.angle() - startValue) * correctionStrength lSpeed = speed else: lSpeed = speed rSpeed = speed self.turnLeftMotor(lSpeed) self.turnRightMotor(rSpeed) #cancel if button pressed if any(self.brick.buttons.pressed()): return else: while revs < motor.angle() / 360: # if not driving staright correct it if self.__gyro.angle() - startValue < 0: rSpeed = speed + abs(self.__gyro.angle() - startValue) * correctionStrength lSpeed = speed elif self.__gyro.angle() - startValue > 0: lSpeed = speed + abs(self.__gyro.angle() - startValue) * correctionStrength rSpeed = speed else: lSpeed = speed rSpeed = speed self.turnLeftMotor(-lSpeed) self.turnRightMotor(-rSpeed) # cancel if button pressed if any(self.brick.buttons.pressed()): return else: self.__fRMotor.reset_angle(0) # convert the input (cm) to revs revs = dist / (self.__config['wheelDiameter'] * math.pi) speed = speed * 1.7 * 6 # convert speed form % to deg/min # driving the robot into the desired direction if ang >= 0 and ang <= 45: multiplier = _map(ang, 0, 45, 1, 0) self.__fRMotor.run_angle(speed, revs * 360, Stop.COAST, False) self.__bRMotor.run_angle(speed * multiplier + 1, revs * 360 * multiplier, Stop.COAST, False) self.__fLMotor.run_angle(speed * multiplier + 1, revs * 360 * multiplier, Stop.COAST, False) self.__bLMotor.run_angle(speed, revs * 360, Stop.COAST, True) elif ang >= -45 and ang < 0: multiplier = _map(ang, -45, 0, 0, 1) self.__fRMotor.run_angle(speed * multiplier + 1, revs * 360 * multiplier, Stop.COAST, False) self.__bRMotor.run_angle(speed, revs * 360, Stop.COAST, False) self.__bLMotor.run_angle(speed * multiplier + 1, revs * 360 * multiplier, Stop.COAST, False) self.__fLMotor.run_angle(speed, revs * 360, Stop.COAST, True) elif ang > 45 and ang <= 90: multiplier = _map(ang, 45, 90, 0, 1) self.__fRMotor.run_angle(speed, revs * 360, Stop.COAST, False) self.__bRMotor.run_angle(speed * multiplier + 1, revs * -360 * multiplier, Stop.COAST, False) self.__fLMotor.run_angle(speed * multiplier + 1, revs * -360 * multiplier, Stop.COAST, False) self.__bLMotor.run_angle(speed, revs * 360, Stop.COAST, True) elif ang < -45 and ang >= -90: multiplier = _map(ang, -45, -90, 0, 1) self.__fRMotor.run_angle(speed * multiplier + 1, revs * -360 * multiplier, Stop.COAST, False) self.__bRMotor.run_angle(speed, revs * 360, Stop.COAST, False) self.__bLMotor.run_angle(speed * multiplier + 1, revs * -360 * multiplier, Stop.COAST, False) self.__fLMotor.run_angle(speed, revs * 360, Stop.COAST, True) elif ang > 90 and ang <= 135: multiplier = _map(ang, 90, 135, 1, 0) self.__fRMotor.run_angle(speed * multiplier + 1, revs * 360 * multiplier, Stop.COAST, False) self.__bRMotor.run_angle(speed, revs * -360, Stop.COAST, False) self.__bLMotor.run_angle(speed * multiplier + 1, revs * 360 * multiplier, Stop.COAST, False) self.__fLMotor.run_angle(speed, revs * -360, Stop.COAST, True) elif ang < -90 and ang >= -135: multiplier = _map(ang, -90, -135, 1, 0) self.__fRMotor.run_angle(speed, revs * -360, Stop.COAST, False) self.__bRMotor.run_angle(speed * multiplier + 1, revs * 360 * multiplier, Stop.COAST, False) self.__fLMotor.run_angle(speed * multiplier + 1, revs * 360 * multiplier, Stop.COAST, False) self.__bLMotor.run_angle(speed, revs * -360, Stop.COAST, True) elif ang > 135 and ang <= 180: multiplier = _map(ang, 135, 180, 0, 1) self.__fRMotor.run_angle(speed * multiplier + 1, revs * -360 * multiplier, Stop.COAST, False) self.__bRMotor.run_angle(speed, revs * -360, Stop.COAST, False) self.__bLMotor.run_angle(speed * multiplier + 1, revs * -360 * multiplier, Stop.COAST, False) self.__fLMotor.run_angle(speed, revs * -360, Stop.COAST, True) elif ang < -135 and ang >= -180: multiplier = _map(ang, -135, -180, 0, 1) self.__fRMotor.run_angle(speed, revs * -360, Stop.COAST, False) self.__bRMotor.run_angle(speed * multiplier + 1, revs * -360 * multiplier, Stop.COAST, False) self.__fLMotor.run_angle(speed * multiplier + 1, revs * -360 * multiplier, Stop.COAST, False) self.__bLMotor.run_angle(speed, revs * -360, Stop.COAST, True) def intervall(self, speed, dist, count): ''' Drives forwads and backwards x times. Args: speed (int): the speed to drive at revs (int): the distance (in cm) to drive count (int): how many times it should repeat the driving ''' # convert the input (cm) to revs revs = dist / (self.__config['wheelDiameter'] * math.pi) / 2 speed = speed * 1.7 * 6 # speed in deg/s to % # move count times forwards and backwards for i in range(count + 1): if self.__config['robotType'] == 'NORMAL': ang = self.__lMotor.angle() # drive backwards self.__rMotor.run_angle(speed, revs * -360, Stop.BRAKE, False) self.__lMotor.run_angle(speed, revs * -360, Stop.BRAKE, False) # return to cancel if any button is pressed while self.__lMotor.angle() > revs * -360: if any(self.brick.buttons.pressed()): return # drive forwards self.__lMotor.run_angle(speed, revs * 360, Stop.BRAKE, False) self.__rMotor.run_angle(speed, revs * 360, Stop.BRAKE, False) # return to cancel if any button is pressed while self.__rMotor.angle() <= ang: if any(self.brick.buttons.pressed()): return elif self.__config['robotType'] == 'ALLWHEEL' or self.__config[ 'robotType'] == 'MECANUM': ang = self.__lMotor.angle() # drive backwards self.__fRMotor.run_angle(speed, revs * -360, Stop.BRAKE, False) self.__bRMotor.run_angle(speed, revs * -360, Stop.BRAKE, False) self.__fLMotor.run_angle(speed, revs * -360, Stop.BRAKE, False) self.__bLMotor.run_angle(speed, revs * -360, Stop.BRAKE, False) # return to cancel if any button is pressed while self.__lMotor.angle() > revs * -360: if any(self.brick.buttons.pressed()): return # drive forwards self.__fRMotor.run_angle(speed, revs * 360, Stop.BRAKE, False) self.__bRMotor.run_angle(speed, revs * 360, Stop.BRAKE, False) self.__fLMotor.run_angle(speed, revs * 360, Stop.BRAKE, False) self.__bLMotor.run_angle(speed, revs * 360, Stop.BRAKE, False) # return to cancel if any button is pressed while self.__rMotor.angle() <= ang: if any(self.brick.buttons.pressed()): return def curve(self, speed, dist, deg): ''' Drives forwads and backwards x times. Args: speed (int): the speed to drive at revs1 (int): the distance (in motor revolutions) for the outer wheel to drive deg (int): how much of a circle it should drive ''' speed = speed * 1.7 * 6 # speed to deg/s from % # gyro starting point startValue = self.__gyro.angle() revs1 = dist / (self.__config['wheelDiameter'] * math.pi) # claculate revs for the second wheel pathOutside = self.__config['wheelDiameter'] * math.pi * revs1 rad1 = pathOutside / (math.pi * (deg / 180)) rad2 = rad1 - self.__config['wheelDistance'] pathInside = rad2 * math.pi * (deg / 180) revs2 = pathInside / (self.__config['wheelDiameter'] * math.pi) # claculate the speed for the second wheel relation = revs1 / revs2 speedSlow = speed / relation if deg > 0: # asign higher speed to outer wheel lSpeed = speed rSpeed = speedSlow self.__rMotor.run_angle(rSpeed, revs2 * 360, Stop.COAST, False) self.__lMotor.run_angle(lSpeed, revs1 * 360 + 5, Stop.COAST, False) #turn while self.__gyro.angle() - startValue < deg and not any( self.brick.buttons.pressed()): pass else: # asign higher speed to outer wheel lSpeed = speed rSpeed = speedSlow self.__rMotor.run_angle(rSpeed, revs2 * 360 + 15, Stop.COAST, False) self.__lMotor.run_angle(lSpeed, revs1 * 360, Stop.COAST, False) #turn while self.__gyro.angle() - startValue > deg and not any( self.brick.buttons.pressed()): pass def toColor(self, speed, color, side): ''' Drives forward until the given colorSensor sees a given color. Args: speed (int): the speed to drive at color (int): the color to look for (0 = Black, 1 = White) side (int): which side's color sensor should be used ''' # sets color to a value that the colorSensor can work with if color == 0: color = Color.BLACK else: color = Color.WHITE # Refactor code # only drive till left colorSensor if side == 2: # if drive to color black drive until back after white to not recognize colors on the field as lines if color == Color.BLACK: while lLight.color() != Color.WHITE and not any( self.brick.buttons.pressed()): self.turnBothMotors(speed) while lLight.color() != color and not any( self.brick.buttons.pressed()): self.turnBothMotors(speed) # only drive till right colorSensor elif side == 3: # if drive to color black drive until back after white to not recognize colors on the field as lines if color == Color.BLACK: while rLight.color() != Color.WHITE and not any( self.brick.buttons.pressed()): self.turnBothMotors(speed) while rLight.color() != color and not any( self.brick.buttons.pressed()): self.turnBothMotors(speed) # drive untill both colorSensors elif side == 23: rSpeed = speed lSpeed = speed rWhite = False lWhite = False while (rLight.color() != color or lLight.color() != color ) and not any(self.brick.buttons.pressed()): #if drive to color black drive until back after white to not recognize colors on the field as lines if color == Color.BLACK: if rLight.color() == Color.WHITE: rWhite = True if lLight.color() == Color.WHITE: lWhite = True self.__rMotor.dc(rSpeed) self.__lMotor.dc(lSpeed) # if right at color stop right Motor if rLight.color() == color and rWhite: rSpeed = 0 # if left at color stop left Motor if lLight.color() == color and lWhite: lSpeed = 0 def toWall(self, speed, *args): ''' Drives until a pressure sensor is pressed Args: speed (int): the speed to drive at ''' while not touch.pressed(): self.turnBothMotors(-abs(speed)) if any(self.brick.buttons()): break self.turnBothMotors(0) def action(self, speed, revs, port): ''' Doesn't drive the robot, but drives the action motors Args: speed (int): the speed to turn the motor at revs (int): how long to turn the motor for port (int): which one of the motors should be used ''' speed = abs(speed) * 1.7 * 6 # speed to deg/s from % if self.__config['useGearing']: self.__gearingPortMotor.run_target(300, port * 90, Stop.HOLD, True) # select gearing Port ang = self.__gearingTurnMotor.angle() self.__gearingTurnMotor.run_angle(speed, revs * 360, Stop.BRAKE, False) # start turning the port # cancel, if any brick button is pressed if revs > 0: while self.__gearingTurnMotor.angle() < revs * 360 - ang: if any(self.brick.buttons.pressed()): self.__gearingTurnMotor.dc(0) return else: while self.__gearingTurnMotor.angle() > revs * 360 + ang: if any(self.brick.buttons.pressed()): self.__gearingTurnMotor.dc(0) return else: # turn motor 1 if port == 1: ang = self.__aMotor1.angle() self.__aMotor1.run_angle(speed, revs * 360, Stop.HOLD, False) if revs > 0: while self.__aMotor1.angle() < revs * 360 - ang: if any(self.brick.buttons.pressed()): self.__aMotor1.dc(0) return else: while self.__aMotor1.angle() > revs * 360 + ang: if any(self.brick.buttons.pressed()): self.__aMotor1.dc(0) return # turm motor 2 elif port == 2: ang = self.__aMotor2.angle() self.__aMotor2.run_angle(speed, revs * 360, Stop.HOLD, False) if revs > 0: while self.__aMotor2.angle() < revs * 360 - ang: if any(self.brick.buttons.pressed()): self.__aMotor2.dc(0) return else: while self.__aMotor2.angle() > revs * 360 + ang: if any(self.brick.buttons.pressed()): self.__aMotor2.dc(0) return def turnLeftMotor(self, speed): ''' Sub-method for driving the left Motor(s) Args: speed (int): the speed to drive the motor at ''' if self.__config['robotType'] == 'NORMAL': self.__lMotor.dc(speed) else: self.__fLMotor.dc(speed) self.__bLMotor.dc(speed) def turnRightMotor(self, speed): ''' Sub-method for driving the right Motor(s) Args: speed (int): the speed to drive the motor at ''' if self.__config['robotType'] == 'NORMAL': self.__rMotor.dc(speed) else: self.__fRMotor.dc(speed) self.__bRMotor.dc(speed) def turnBothMotors(self, speed): ''' Submethod for setting a motor.dc() to all motors Args: speed (int): the speed (in percent) to set the motors to ''' if self.__config['robotType'] == 'NORMAL': self.__rMotor.dc(speed) self.__lMotor.dc(speed) else: self.__fRMotor.dc(speed) self.__bRMotor.dc(speed) self.__fLMotor.dc(speed) self.__bLMotor.dc(speed) def breakMotors(self): '''Sub-method for breaking all the motors''' if self.__config['robotType'] == 'NORMAL': self.__lMotor.hold() self.__rMotor.hold() else: self.__fRMotor.hold() self.__bRMotor.hold() self.__fLMotor.hold() self.__bLMotor.hold() time.sleep(0.2) def _map(self, x, in_min, in_max, out_min, out_max): ''' Converts a given number in the range of two numbers to a number in the range of two other numbers Args: x (int): the input number that should be converted in_min (int): The minimal point of the range of input number in_max (int): The maximal point of the range of input number out_min (int): The minimal point of the range of output number out_max (int): The maximal point of the range of output number Returns: int: a number between out_min and out_max, de ''' return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min def getGyroAngle(self): return self.__gyro.angle() def setRemoteValues(self, data): x = data['x'] y = data['y'] if x == 0: x = 0.0001 if data['y'] == 0 and data['x'] == 0: self.breakMotors() else: radius = int(math.sqrt(x**2 + y**2)) # distance from center ang = math.atan(y / x) # angle in radians a = int(self._map(radius, 0, 180, 0, 100)) b = int(-1 * math.cos(2 * ang) * self._map(radius, 0, 180, 0, 100)) if x < 0: temp = a a = b b = temp if y < 0: temp = a a = -b b = -temp self.turnLeftMotor(int(self._map(a, 0, 100, 0, data['maxSpeed']))) self.turnRightMotor(int(self._map(b, 0, 100, 0, data['maxSpeed']))) if data['a1'] == 0: self.__aMotor1.hold() else: a1Speed = data['a1'] self.__aMotor1.dc(a1Speed)
#!/usr/bin/env pybricks-micropython from pybricks import ev3brick as brick from pybricks.ev3devices import Motor from pybricks.parameters import Port, Button, Stop from pybricks.tools import print, wait motor = Motor(Port.B) while True: bts = brick.buttons() if Button.RIGHT in bts: motor.run_angle(200, 500, Stop.COAST, False) elif Button.CENTER in bts: break print(motor.speed(), motor.angle()) wait(100)
targetAngle = 365 # how fast do we want to get there? speed = 100 # make our motor objects leftMotor = Motor(Port.B) rightMotor = Motor(Port.C) # set them all to read zero leftMotor.reset_angle(0) rightMotor.reset_angle(0) # read the left motor's current position # this should just be zero. angle = leftMotor.angle() # start moving the robot straight leftMotor.run(speed) rightMotor.run(speed) # keep going until the left motor has rotated # past our target angle while angle < targetAngle: angle = leftMotor.angle() # here we can change the speed! rampSpeed = speed leftMotor.run(rampSpeed) rightMotor.run(rampSpeed) # now we can stop!
# robot = DriveBase(leftmotor, rightmotor, diameter, axletrack) c = ColorSensor(Port.S3) t = TouchSensor(Port.S1) SPEED = 150 # deg / second MIDPOINT = 55 CORRECTION = -7 INTERVAL = 0.05 # seconds leftmotor.reset_angle(0) rightmotor.reset_angle(0) DIST = 2000 while not t.pressed() and (leftmotor.angle() + rightmotor.angle() < DIST): cv = c.reflection() mismatch = cv - MIDPOINT direction = mismatch * CORRECTION if direction > SPEED: over = direction - SPEED leftmotor.run(SPEED) rightmotor.run(-over) elif direction > 0: leftmotor.run(SPEED) rightmotor.run(SPEED - direction) elif direction > -SPEED: leftmotor.run(SPEED + direction) rightmotor.run(SPEED) else:
from pybricks.robotics import DriveBase from pybricks.media.ev3dev import SoundFile, ImageFile import math #import my functions from Newton_Model import newton_hit_the_ball ################# ev3 = EV3Brick() ev3.speaker.beep() ################# # initialize motors and sensors hitter = Motor(Port.D, Direction.COUNTERCLOCKWISE) arm_start = hitter.angle() eyes = UltrasonicSensor(Port.S1) button = TouchSensor(Port.S4) #------General functions------ def average_distance(): avgd = 0 n = 10 for i in range(0,n): avgd += eyes.distance()/1000 avgd = avgd/n return avgd
return self.seesACliff or self.seesAWall #Init motors motorA = Motor(Port.A) motorB = Motor(Port.B) motorD = Motor(Port.D) #Init sensors ultrasonicDown = UltrasonicSensor(Port.S1) ultrasonicFront = UltrasonicSensor(Port.S2) gyrosensor = GyroSensor(Port.S4) gyrosensor.reset_angle(0) #Init data container robotData = RobotData(DRIVE_MODE_STOP, True, motorD.angle()) #Drives the vehicle at a set speed def Drive(speed): motorA.run(speed) motorB.run(speed) #Drives the vehicle at a speed for a time def DriveTimed(speed, time): motorA.run_time(speed, MOVEMENT_DURATION) motorB.run_time(speed, MOVEMENT_DURATION) #Turns the vehicle at a speed
else: client.publish(MQTT_Topic_Train, TRAIN_FORW) Train = TRAIN_FORW NewTrain = True else: # train is moving away so don't care pass else: # train is already stopped so don't care pass # check for new Speed setting if nSettle > 0: nSettle = nSettle - 1 if nSettle == 0: client.publish(MQTT_Topic_Speed, str(round(m.angle() / SCALE))) else: pass else: curPosition = m.angle() delta = abs(Speed * SCALE - curPosition) if delta > TOLERANCE: # new speed value received or user rotated the motor axle if NewSpeed: m.run_target(MOTORSpeed, round(Speed * SCALE), Stop.COAST) NewSpeed = False else: nSettle = SETTLE # wait user choice to settle else: NewSpeed = False
total_rotation = int(args.rotations) * 360 rotation_speed = int(args.speed) log_interval = int(args.interval) # Run the platform motor platform_motor.run_angle(speed=rotation_speed, rotation_angle=total_rotation, then=Stop.HOLD, wait=False) wait(100) # print('Platform Speed: {0}'.format(platform_motor.speed())) # Log the time and the sensor readings 10 times while platform_motor.speed() > 0: rotation_angle = platform_motor.angle() character_reflectivity = platform_color_sensor.reflection() print("Angle: {0} degrees, Reflectivity: {1}%".format( platform_motor.angle(), platform_color_sensor.reflection())) data.log(rotation_angle, character_reflectivity) # Wait some time so the motor can move a bit wait(log_interval) # Play another beep sound. ev3.speaker.beep(frequency=1000, duration=500) break else: numbers = [
ymotor = Motor(Port.B, Direction.CLOCKWISE) xmotor = Motor(Port.A, Direction.COUNTERCLOCKWISE) direction = 1 uart = UARTDevice(Port.S3, 9600, timeout=10000) wait(500) uart.write('a') print("Done") curr = "" previous = 'q' uart.write('q') while True: xangle = xmotor.angle() yangle = ymotor.angle() print(str(xangle) + " " + str(yangle)) if yangle > 20 and xangle > 20: curr = 'x' elif yangle > 20 and (xangle < 20 and xangle > -20): curr = 'y' elif yangle > 20 and (xangle < -20): curr = 'z' elif (yangle < 20 and yangle > -20) and xangle < -20: curr = 's' elif (yangle < -20) and xangle < -20: curr = 't' elif (yangle < -20) and (xangle < 20 and xangle > -20): curr = 'u' elif (yangle < -20) and (xangle > 20):
average_control_loop_period = TARGET_LOOP_PERIOD / 1000 control_loop_timer.reset() else: average_control_loop_period = (control_loop_timer.time() / 1000 / control_loop_count) control_loop_count += 1 # calculate robot body angle and speed gyro_sensor_value = gyro_sensor.speed() gyro_offset *= (1 - GYRO_OFFSET_FACTOR) * gyro_offset gyro_offset += GYRO_OFFSET_FACTOR * gyro_sensor_value robot_body_rate = gyro_sensor_value - gyro_offset robot_body_angle += robot_body_rate * average_control_loop_period # calculate wheel angle and speed left_motor_angle = left_motor.angle() right_motor_angle = right_motor.angle() previous_motor_sum = motor_position_sum motor_position_sum = left_motor_angle + right_motor_angle change = motor_position_sum - previous_motor_sum motor_position_change.insert(0, change) del motor_position_change[-1] wheel_angle += change - drive_speed * average_control_loop_period wheel_rate = sum( motor_position_change) / 4 / average_control_loop_period # This is the main control feedback calculation. output_power = (-0.01 * drive_speed) + ( 0.8 * robot_body_rate + 15 * robot_body_angle + 0.08 * wheel_rate + 0.12 * wheel_angle) if output_power > 100:
class MbMotor(): """ Control a motor, besides the fact that you can detect if a motor got stalled the main reason for this class is to solve a bug for pybricks.ev3devices.Motor. The bug is that when you set the motor to move in a Direction.COUNTERCLOCKWISE sometimes it failes to detect it. This class is made on top of pybricks.ev3devices.Motor Args: port (Port): The port of the device clockwise_direction (bool): Sets the defualt movement of the motor clockwise or counterclockwise, True for clockwise else counterclockwise exit_exec (Function): Function that returns True or False, the motor will stop if returns True """ def __init__(self, port, clockwise_direction=True, exit_exec=lambda: False): self.core = Motor(port) self.port = port self.direction = 1 if clockwise_direction else -1 self.exit_exec = exit_exec def angle(self): """ Get the distance the robot has moved in degrees Returns: angle (int): The distance the robot has moved in degrees """ return self.core.angle() * self.direction def speed(self): """ Get the speed of the motor Returns: speed (int): The current speed of the motor """ return self.core.speed() * self.direction def stalled(self, min_speed=0): if abs(self.speed()) <= abs(min_speed): return True return False def run_angle(self, speed, angle, wait=True, detect_stall=False): """ Run the motor to a specific angle Args: speed (int): The speed of the motor angle (int): Degrees to run the motor at wait (bool): Sets if the robot is going to stop for the motor to complete this method or not """ def exec(self, speed, angle): moved_enough = False self.reset_angle() self.run(speed) while True: if abs(self.angle()) > 50: moved_enough = True if moved_enough and detect_stall: if self.stalled(): break if abs(self.angle()) > abs(angle) or self.exit_exec(): break self.hold() if wait: exec(self, speed, angle) else: threading.Thread(target=exec, args=[self, speed, angle]).start() def run_time(self, speed, msec, wait=True): """ Run the motor to a amount of time Args: speed (int): The speed of the motor msec (int): Time to move the robot wait (bool): Sets if the robot is going to stop for the motor to complete this method or not """ def exec(self, speed, msec): self.reset_angle() self.run(speed) s = time() while True: if round(time() - s, 2) * 1000 >= abs(msec) or self.exit_exec(): break self.hold() if wait: exec(self, speed, msec) else: threading.Thread(target=exec, args=[self, speed, msec]).start() def run(self, speed): """ Run the motor to a constant speed Args: speed (int): Speed to run at Note: speed parameter should be between -800 and 800 """ self.core.run(speed * self.direction) def dc(self, speed): """ Run the motor to a constant speed Args: speed (int): Speed to run at Note: speed parameter should be between -100 and 100 """ self.core.dc(speed * self.direction) def hold(self): """ Stop the motor and hold its position """ self.core.hold() def brake(self): """ Passively stop the motor """ self.core.brake() def stop(self): """ No current is being aplied to the robot, so its gonna stop due to friction """ self.core.stop() def reset_angle(self, angle=0): """ Set the motor angle Args: angle (int): Angle to set the motor at """ self.core.reset_angle(angle) def is_stalled(self, min_speed=0): """ Check if the motor got stalled Args: min_speed (int): The minim speed the motor should be moving at """ if abs(self.speed()) <= abs(min_speed): return True return False def __repr__(self): return "Motor Properties:\nPort: " + str( self.port) + "\nDefault Direction: " + str(self.direction)
#!/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 wait, StopWatch from pybricks.robotics import DriveBase from time import sleep import ubinascii, ujson, urequests, utime xmotor = Motor(Port.A) ymotor = Motor(Port.B) with open('error.csv', 'a') as f: while True: f.write(str(xmotor.angle()) + ', ' + str(ymotor.angle()) + '\n') wait(.01)
#htSide = Ev3devSensor(Port.S2) colLeft = ColorSensor(Port.S3) colRight = ColorSensor(Port.S4) except: pass stopwatch = StopWatch() LineTrack = PID_LineTrack(leftMotor, rightMotor, 0.3, 0, 5) Straight = PID_Straight(leftMotor, rightMotor, 0.5, 0, 10) # Write your program here. ev3.speaker.beep() watchtime = stopwatch.time() resetMotor(leftMotor, rightMotor) while rightMotor.angle() < 100: Straight.move(80)= stop(leftMotor, rightMotor) while colRight.reflection() > 10: LineTrack.track(80, 40) stop(leftMotor, rightMotor) resetMotor(leftMotor, rightMotor) while rightMotor.angle() < 200: Straight.move(80) stop(leftMotor, rightMotor)
#l_motor will find the white line behind the black line l_motor.run_time(-50,800) l_motor.reset_angle(0) while True: if l_color.ambient()>5: r_motor.run_time(0,0) l_motor.run_time(0,0) ev3.screen.print(l_color.ambient()) wait(500) ev3.screen.print() break else: l_motor.run_time(-50,100) ev3.screen.print(l_motor.angle()) wait(50) n = l_motor.angle() l_motor.run_target(50, -n/2) ev3.screen.print(l_color.ambient()) wait(50) #r_motor while True: if r_color.ambient()>5: r_motor.run_time(0,0) l_motor.run_time(0,0) ev3.screen.print(r_color.ambient()) wait(50) ev3.screen.print() break
class Rac3Truck: WHEEL_DIAMETER = 30 # milimeters AXLE_TRACK = 120 # milimeters def __init__( self, left_motor_port: str = Port.B, right_motor_port: str = Port.C, polarity: str = 'inversed', steer_motor_port: str = Port.A, ir_sensor_port: str = Port.S4, ir_beacon_channel: int = 1): if polarity == 'normal': self.left_motor = Motor(port=left_motor_port, positive_direction=Direction.CLOCKWISE) self.right_motor = Motor(port=right_motor_port, positive_direction=Direction.CLOCKWISE) else: self.left_motor = \ Motor(port=left_motor_port, positive_direction=Direction.COUNTERCLOCKWISE) self.right_motor = \ Motor(port=right_motor_port, positive_direction=Direction.COUNTERCLOCKWISE) self.driver = DriveBase(left_motor=self.left_motor, right_motor=self.right_motor, wheel_diameter=self.WHEEL_DIAMETER, axle_track=self.AXLE_TRACK) self.steer_motor = Motor(port=steer_motor_port, positive_direction=Direction.CLOCKWISE) self.ir_sensor = InfraredSensor(port=ir_sensor_port) self.ir_beacon_channel = ir_beacon_channel def reset(self): self.steer_motor.run_time( speed=300, time=1500, then=Stop.COAST, wait=True) self.steer_motor.run_angle( speed=-500, rotation_angle=120, then=Stop.HOLD, wait=True) self.steer_motor.reset_angle(angle=0) def steer_left(self): if self.steer_motor.angle() > -65: self.steer_motor.run_target( speed=-200, target_angle=-65, then=Stop.HOLD, wait=True) else: self.steer_motor.hold() def steer_right(self): if self.steer_motor.angle() < 65: self.steer_motor.run_target( speed=200, target_angle=65, then=Stop.HOLD, wait=True) else: self.steer_motor.hold() def steer_center(self): if self.steer_motor.angle() < -7: self.steer_motor.run_target( speed=200, target_angle=4, then=Stop.HOLD, wait=True) elif self.steer_motor.angle() > 7: self.steer_motor.run_target( speed=-200, target_angle=-4, then=Stop.HOLD, wait=True) self.steer_motor.hold() wait(100) def drive_by_ir_beacon(self): ir_beacon_button_pressed = \ set(self.ir_sensor.buttons(channel=self.ir_beacon_channel)) # forward if ir_beacon_button_pressed == {Button.LEFT_UP, Button.RIGHT_UP}: self.driver.drive( speed=800, turn_rate=0) self.steer_center() # backward elif ir_beacon_button_pressed == {Button.LEFT_DOWN, Button.RIGHT_DOWN}: self.driver.drive( speed=-800, turn_rate=0) self.steer_center() # turn left forward elif ir_beacon_button_pressed == {Button.LEFT_UP}: self.left_motor.run(speed=600) self.right_motor.run(speed=1000) self.steer_left() # turn right forward elif ir_beacon_button_pressed == {Button.RIGHT_UP}: self.left_motor.run(speed=1000) self.right_motor.run(speed=600) self.steer_right() # turn left backward elif ir_beacon_button_pressed == {Button.LEFT_DOWN}: self.left_motor.run(speed=-600) self.right_motor.run(speed=-1000) self.steer_left() # turn right backward elif ir_beacon_button_pressed == {Button.RIGHT_DOWN}: self.left_motor.run(speed=-1000) self.right_motor.run(speed=-600) self.steer_right() # otherwise stop else: self.driver.stop() self.steer_center()
if (car2Position_Y >= 126): car2Position_Y = -42 car2Position_X = randint(1, 3) * 60 while True: brick.display.image("car.jpg", (playerPosition_X, playerPosition_Y), clear=False) brick.display.image("carEnemy.jpg", (car1Position_X, car1Position_Y), clear=False) brick.display.image("carEnemy.jpg", (car2Position_X, car2Position_Y), clear=False) brick.display.image("carEnemy.jpg", (car3Position_X, car3Position_Y), clear=False) if (motor.angle() >= -30 and motor.angle() <= 30): playerPosition_X = 60 playerPositionNow = 1 elif (motor.angle() >= 30): playerPosition_X = 120 playerPositionNow = 2 elif (motor.angle() <= -30): playerPosition_X = 0 playerPositionNow = 0 if (playerPositionThen != playerPositionNow): playerPositionThen = playerPositionNow brick.sound.beep() updatePositionCarEnemy()
#!/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 import time brick.sound.beep() motor = Motor(Port.B) # motor.angle() # get angle (int) # motor.run(speed) # speed: deg/s # motor.run_angle() while True: print(motor.angle()) wait(10)
#!/usr/bin/env pybricks-micropython from pybricks import ev3brick as brick from pybricks.ev3devices import Motor, TouchSensor, UltrasonicSensor from pybricks.parameters import Port, Direction from pybricks.tools import print, wait from pybricks.robotics import DriveBase center = Motor(Port.D) print(center.angle())