# LEGO type:standard slot:6 autostart import math from spike import PrimeHub, Motor, MotorPair, ColorSensor, MotionSensor, DistanceSensor from spike.control import wait_for_seconds print("Lade IQ-Bibliothek") ''' 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 = "C" FRONT_MOTOR_LEFT_PORT = "D" #self.farbSensor: ColorSensor = ColorSensor("C") self.bothFrontMotors: MotorPair = MotorPair(FRONT_MOTOR_LEFT_PORT, FRONT_MOTOR_RIGHT_PORT) self.linker_motor_vorne: Motor = Motor(FRONT_MOTOR_LEFT_PORT) self.linker_motor_vorne.set_stall_detection(stop_when_stalled=True) self.rechter_motor_vorne: Motor = Motor(FRONT_MOTOR_RIGHT_PORT) self.rechter_motor_vorne.set_stall_detection(stop_when_stalled=True) 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() #try: # # self.abstandsSensor: DistanceSensor = DistanceSensor("D") #except: # self.abstandsSensor: DistanceSensor = DistanceSensor("C") 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): ''' Gibt die gefahrene Strecke basierend auf den Radumdrehungen zurück ''' return -self.linker_motor.get_degrees_counted()/360 * self.rad_umfang def drehe(self, grad=90, with_reset=True, speed=10): """ 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() # Gierwinkel zurücksetzen #steering = 100 if grad > 0 else -100 toleranz = 0 # Toleranz soll null sein. Kann erhöht werden, falls der Roboter sich unendlich dreht. aktuell = self.bewegungsSensor.get_yaw_angle() # Aktuelle Position ziel = grad steering = 100 if ziel > aktuell else -100 self.antrieb.start(steering=steering, speed=speed) # Mit bestimmer Geschwindigkeit starten 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 speed = speed * richtung # Die Geschwindigkeit soll negativ sein, wenn wir rückwärts fahren # Alles zurücksetzen self.linker_motor.set_degrees_counted(0) self.bewegungsSensor.reset_yaw_angle() # Mit irgendeiner Geschwindigkeit g>0 starten. Wert ist irrelevant #self.antrieb.start_tank(10, 10) self.antrieb.set_default_speed(10) ziel_speed=speed k_speed=0.04 # Beschleunigung in % # Regler Parameter: Nicht ändern! kp = 5.0 # Verstärkungsfaktor zur Regelung ki = 0.5 # Integralfaktor zur Regelung sum_cm = 0 # bereits gefahrene Strecke versatz = 0 # aktueller Versatz sum_versatz = 0 # Summe des Versatzes über Zeit self.antrieb.start_tank_at_power(50, 50) speed = min(20, speed) # wiederhole solange die gefahrene Strecke noch nicht erreicht ist while sum_cm < cm * richtung: wait_for_seconds(0.05) # Sonst wird das zu oft ausgeführt speed = ziel_speed * k_speed + speed * (1 - k_speed) sum_cm = self.strecke_gefahren() * richtung # Gefahrene Strecke, ggf. eben negativ machen versatz = self.bewegungsSensor.get_yaw_angle() # Um wie viel sind wir falsch? sum_versatz = sum_versatz + versatz abweichung = (kp * versatz + ki * sum_versatz) / 100 # Abweichung berechnen, um zu korrigieren linker_speed = speed * (1 - abweichung * richtung) rechter_speed = speed * (1 + abweichung * richtung) self.antrieb.start_tank_at_power(int(linker_speed), int(rechter_speed)) # Mit neuer Geschwindigkeit starten print("Versatz: " + str(versatz) + " , linker Speed: " + str(linker_speed) + ", rechter Speed: " + str(rechter_speed) + ", strecke: " + str(sum_cm) + ", sum versatz: " + str(sum_versatz)) self.antrieb.stop() # Stoppen self.drehe(-versatz) # Da Versatz immer != 0, korrigieren 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) #deprecated def fahre_gerade_aus_alt(self, cm: float, speed: int): """ Funktion zum gerade aus fahren mit Korrektur am Ende Wird nicht mehr aktiv genutzt, da wir jetzt fahre_gerade_aus haben, welche geregelt ist, und der Roboter daher nicht schief wird. :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) # TODO: Geregeltes Fahren ist noch nicht eingebaut def fahre_bis_abstand(self, abstand: int, speed=30, geregelt=True): """ Nutzt den Abstandssensor, um zu fahren, bis ein Abstand erreicht ist :param abstand: Abstand zum Objekt :param speed: Geschwindigkeit, mit der gefahren wird :param geregelt: Soll mit Regler gefahren werden? """ 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)) def schaufel(self, rotations, speed): self.linker_motor_vorne.run_for_rotations(rotations, speed) print("Fertig geladen.")