From a6ea00afb1d983902dc084296ca86c22a9860887 Mon Sep 17 00:00:00 2001 From: "(MakerLab) Laptop 2" Date: Wed, 3 Jan 2024 16:58:32 +0100 Subject: [PATCH] =?UTF-8?q?Funktion=20f=C3=BCr=20neue=20Schaufel=20und=20D?= =?UTF-8?q?oku?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- iqrobot.py | 130 ++++++++++++++++++++++++++++++++++++++++------------- main.py | 20 +++------ 2 files changed, 105 insertions(+), 45 deletions(-) diff --git a/iqrobot.py b/iqrobot.py index d2750f3..3056463 100644 --- a/iqrobot.py +++ b/iqrobot.py @@ -5,7 +5,8 @@ import math from spike import PrimeHub, Motor, MotorPair, ColorSensor, MotionSensor, DistanceSensor from spike.control import wait_for_seconds -HELLO = "HELLO IQ" +print("Lade IQ-Bibliothek") + ''' Wir nutzen "Duck typing", dh wir schreiben hinter jede Variabel mit ':' die Klasse, zB `leftMotor: Motor` @@ -26,6 +27,10 @@ class IQRobot: FRONT_MOTOR_LEFT_PORT = "A" 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) @@ -52,62 +57,99 @@ class IQRobot: 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): + + 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() + self.bewegungsSensor.reset_yaw_angle() # Gierwinkel zurücksetzen + #steering = 100 if grad > 0 else -100 - toleranz = 0 - aktuell = self.bewegungsSensor.get_yaw_angle() + 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=10) + 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): - self.linker_motor.set_degrees_counted(0) + """ + 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) - linker_speed=speed - rechter_speed=speed - kp = 1.5 - ki = 1.0 - sum_cm = 0 - sum_versatz = 0 - while sum_cm < cm: - wait_for_seconds(0.05) - sum_cm = self.strecke_gefahren() - versatz = self.bewegungsSensor.get_yaw_angle() + linker_speed=speed # Geschwindigkeit linker Motor + rechter_speed=speed # 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) # Sonst wird das zu oft ausgeführt + 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 - linker_speed = speed * (1 - abweichung) - rechter_speed = speed * (1 + abweichung) - self.antrieb.start_tank_at_power(int(linker_speed), int(rechter_speed)) + 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)) - self.antrieb.stop() - self.drehe(-versatz) + self.antrieb.stop() # Stoppen + self.drehe(-versatz) # Da Versatz immer != 0, korrigieren def fahre_mit_drehung(self, strecke1, grad, strecke2): @@ -119,11 +161,14 @@ class IQRobot: 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 @@ -142,17 +187,38 @@ class IQRobot: 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): + + def heber(self, cm,speed): + """ + Lässt den Heber fahren + + :param speed: Geschwindigkeit, mit der der Heber bewegt wird + :param cm: Um wie viel soll der Heber bewegt werden? + """ + self.bothFrontMotors.move_tank(-cm*3.3,"cm", -speed, speed) # Heber bewegen + + + def schaufel(self,prozent, speed=20): + """ + Lässt die Schaufel fahren + + :param prozent: Auf wie viel Prozent soll die Schaufel bewegt werden? + """ volle_umdrehung=0.29 rotations=volle_umdrehung*prozent/100 - self.bothFrontMotors.move(rotations, unit='rotations',speed=20) - + #self.bothFrontMotors.move(rotations, unit='rotations',speed=20) + self.bothFrontMotors.move_tank(rotations, 'rotations', speed, -speed) + # 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: @@ -161,7 +227,7 @@ class IQRobot: self.antrieb.stop() -print("successfully loaded the IQ Lego teams code :)") +print("Fertig geladen.") diff --git a/main.py b/main.py index 7154a34..bee3bdd 100644 --- a/main.py +++ b/main.py @@ -27,11 +27,11 @@ def importFile(slotid=0, precompiled=False, module_name='importFile'): with open("/projects/.slots","rt") as f: slots = eval(str(f.read())) print(slots) - print(os.listdir("/projects/"+str(slots[slotid]["id"]))) + #print(os.listdir("/projects/"+str(slots[slotid]["id"]))) with open("/projects/"+str(slots[slotid]["id"])+"/__init__"+suffix,"rb") as f: print("trying to read import program") program = f.read() - print(program) + #print(program) try: os.remove("/"+module_name+suffix) except: @@ -52,14 +52,6 @@ def importFile(slotid=0, precompiled=False, module_name='importFile'): # Dateiname und Modulname sollten gleich sein, dann kann man Code Completion nutzen importFile(slotid=6, precompiled=True, module_name="iqrobot") import iqrobot as iq -print(iq.HELLO) - -# Importiere Go Robot Code -#importFile(slotid=3, precompiled=True, module_name="gorobot") -#import gorobot as gr -#gr.exampleFour() -#gr.db.gyroRotation(90, 25, 35, 25) - ################### Hauptcode #################################### ''' @@ -121,6 +113,8 @@ def augmented_reality(): iqRobot.fahre_gerade_aus(5,20) -druckmaschine() -hologram() -augmented_reality() +#iqRobot.fahre_gerade_aus(16, 20) +#iqRobot.drehe(38) +#iqRobot.fahre_gerade_aus(33,25) +iqRobot.schaufel(1600, speed=100 ) +iqRobot.schaufel(-1600, speed=100 ) \ No newline at end of file