# LEGO type:standard slot:6 autostart import math from spike import PrimeHub, Motor, MotorPair, ColorSensor, MotionSensor, DistanceSensor from spike.control import wait_for_seconds HELLO = "HELLO IQ" ''' Wir nutzen "Duck typing", dh wir schreiben hinter jede Variabel mit ':' die Klasse, zB `leftMotor: Motor` damit man dann später auch wieder Code Completion hat bei Nutzung der Variablen ''' class IQRobot: def __init__(self, hub: PrimeHub): self.hub: PrimeHub = hub # Radantrieb LEFT_MOTOR_PORT = 'E' RIGHT_MOTOR_PORT = 'F' # Motoren für Aufsätze FRONT_MOTOR_RIGHT_PORT = "B" FRONT_MOTOR_LEFT_PORT = "A" self.bothFrontMotors: MotorPair = MotorPair(FRONT_MOTOR_LEFT_PORT, FRONT_MOTOR_RIGHT_PORT) self.linker_motor: Motor = Motor(LEFT_MOTOR_PORT) self.antrieb: MotorPair = MotorPair(LEFT_MOTOR_PORT, RIGHT_MOTOR_PORT) # Radumfang neu berechnen und Motor konfigurieren # Radius der Antriebsräder self.rad_radius = 2.1 # Abstand zwischen Rädern (Mitte) und Vorderseite des Roboters self.abstand_rad_front = 5.55 self.rad_umfang = 2 * math.pi * self.rad_radius self.antrieb.set_motor_rotation(self.rad_umfang) self.bewegungsSensor: MotionSensor = MotionSensor() self.abstandsSensor: DistanceSensor = DistanceSensor("D") def show(self, image: str): ''' Zeige Bild auf LED Matrix des Spikes image: Bildname wie zB 'HAPPY' ''' self.hub.light_matrix.show_image(image) def strecke_gefahren(self): return -self.linker_motor.get_degrees_counted()/360 * self.rad_umfang def drehe(self, grad=90, with_reset=True): """ Funktion um den Roboter auf der Stelle zu drehen :param int grad: Grad um die der Roboter gedreht werden soll mittels Vorzeichen +/- kann links oder rechts herum gedreht werden :param bool with_reset: Parameter, um den Gierwinkel zurückzusetzen, Standard: True """ # ist überhaupt etwas zu tun für uns? d.h. grad ist enweder 0 oder 360 if grad == 0 or grad == 360 : print("nichts zu tun") return # soll der Gierwinkel zurückgesetzt werden? if with_reset: self.bewegungsSensor.reset_yaw_angle() #steering = 100 if grad > 0 else -100 toleranz = 0 aktuell = self.bewegungsSensor.get_yaw_angle() ziel = grad steering = 100 if ziel > aktuell else -100 self.antrieb.start(steering=steering, speed=10) differenz = ziel - aktuell print ("Start Ziel: {}, Aktuell: {}".format(ziel, aktuell)) # wiederhole solange der Grad der Drehung noch nicht erreicht ist while abs(differenz) > toleranz : aktuell = self.bewegungsSensor.get_yaw_angle() differenz = ziel - aktuell pass # stoppe die Bewegung self.antrieb.stop() print ("Final Ziel: {}, Aktuell: {}".format(ziel, aktuell)) def fahre_gerade_aus(self, cm, speed=20): """ Funktion um den Roboter geradeaus fahren zu lassen :param int cm: Strecke in cm, die der Roboter geradeaus fahren soll :param int speed: Geschwindigkeit zum Fahren der Strecke, Standard: 20 """ # ist überhaupt etwas zu tun für uns? d.h. cm = 0 if cm == 0 : print("nichts zu tun") return # wollen wir vorwärts oder rückwarts fahren? richtung = 1 if cm < 0: richtung = -1 self.linker_motor.set_degrees_counted(0) self.bewegungsSensor.reset_yaw_angle() self.antrieb.start_tank(10, 10) self.antrieb.set_default_speed(10) linker_speed=speed * richtung # Geschwindigkeit linker Motor rechter_speed=speed * richtung # Geschwindigkeit rechter Motor kp = 1.5 # Verstärkungsfaktor zur Regelung ki = 1.0 # Integralfaktor zur Regelung sum_cm = 0 # bereits gefahrene Strecke versatz = 0 # aktueller Versatz sum_versatz = 0 # Summe des Versatzes über Zeit # wiederhole solange die gefahrene Strecke noch nicht erreicht ist while sum_cm < cm * richtung: wait_for_seconds(0.05) sum_cm = self.strecke_gefahren() versatz = self.bewegungsSensor.get_yaw_angle() sum_versatz = sum_versatz + versatz abweichung = (kp * versatz + ki * sum_versatz) / 100 linker_speed = speed * (1 - abweichung) rechter_speed = speed * (1 + abweichung) self.antrieb.start_tank_at_power(int(linker_speed), int(rechter_speed)) #print("Versatz: " + str(versatz) + " , linker Speed: " + str(linker_speed) + ", rechter Speed: " + str(rechter_speed) + ", strecke: " + str(sum_cm)) self.antrieb.stop() self.drehe(-versatz) def fahre_mit_drehung(self, strecke1, grad, strecke2): """ Funktion für eine Fahrt mit 1. Strecke, dann Drehung in der Mitte, dann 2. Strecke Vereinfacht die Logik, da der Roboter durch die Drehung einen Versatz hat gegenüber einer Strecke die mit dem Lineal ausgemessen wurde """ self.fahre_gerade_geregelt(strecke1 + self.abstand_rad_front) self.drehe(grad) self.fahre_gerade_geregelt(strecke2 - self.abstand_rad_front) def fahre_gerade_aus_alt(self, cm: float, speed: int): """ Funktion zum gerade aus fahren mit Korrektur am Ende :param int cm: Zentimeter die gerade aus gefahren werden soll :param speed: Geschwindigkeit mit der gefahren wird """ self.bewegungsSensor.reset_yaw_angle() self.antrieb.move_tank(amount=cm,left_speed=speed, right_speed=speed) drehung = self.bewegungsSensor.get_yaw_angle() print(drehung) if drehung > 0: richtung = -1 else: richtung = 1 while abs(drehung) > 2: self.antrieb.move(amount=richtung * 0.1, steering=100) drehung = self.bewegungsSensor.get_yaw_angle() print(drehung) def heber(self, cm,speed): self.bothFrontMotors.move_tank(-cm*3.3,"cm", -speed, speed) def schaufel(self,prozent): volle_umdrehung=0.29 rotations=volle_umdrehung*prozent/100 self.bothFrontMotors.move(rotations, unit='rotations',speed=20) def fahre_bis_abstand(self, abstand: int, speed=30, geregelt=True): self.antrieb.start_at_power(speed) abstand_gerade = self.abstandsSensor.get_distance_cm() while abstand_gerade > abstand: abstand_gerade = self.abstandsSensor.get_distance_cm() print(str(abstand_gerade)) self.antrieb.stop() print("successfully loaded the IQ Lego teams code :)")