diff --git a/iqrobot.py b/iqrobot.py index 749d2f8..b830cd8 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) @@ -50,9 +55,13 @@ class IQRobot: 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): + + def drehe(self, grad=90, with_reset=True, speed=10): """ Funktion um den Roboter auf der Stelle zu drehen @@ -68,14 +77,14 @@ class IQRobot: # 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)) @@ -106,11 +115,13 @@ class IQRobot: richtung = 1 if cm < 0: richtung = -1 - speed = speed * richtung + speed = speed * richtung # Die Geschwindigkeit soll negativ sein, wenn wir rückwärts fahren - self.linker_motor.set_degrees_counted(0) + # 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) @@ -124,18 +135,18 @@ class IQRobot: # wiederhole solange die gefahrene Strecke noch nicht erreicht ist while sum_cm < cm * richtung: - wait_for_seconds(0.05) - sum_cm = self.strecke_gefahren() * richtung - versatz = self.bewegungsSensor.get_yaw_angle() + 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 + 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)) + 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): """ @@ -147,9 +158,13 @@ class IQRobot: 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 @@ -170,14 +185,34 @@ class IQRobot: print(drehung) def heber(self, cm,speed): - self.bothFrontMotors.move_tank(-cm*3.3,"cm", -speed, speed) + """ + Lässt den Heber fahren - def schaufel(self,prozent): + :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: @@ -186,7 +221,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 628ff40..723c52b 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 #################################### ''' @@ -120,6 +112,8 @@ def augmented_reality(): iqRobot.drehe(-90) 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