diff --git a/README.md b/README.md index d7c0858..baba04a 100644 --- a/README.md +++ b/README.md @@ -16,7 +16,7 @@ Importiere den Code entweder über die Shell oder einen Git Client: - für Mac oder Windows: https://www.sourcetreeapp.com/ -Benutzername und Passwort für Makerlab eingeben +Username und PW für Makerlab eingeben Übersicht über Git Commands: https://ndpsoftware.com/git-cheatsheet.html#loc=workspace; diff --git a/iqrobot.py b/iqrobot.py index b830cd8..cec7049 100644 --- a/iqrobot.py +++ b/iqrobot.py @@ -2,11 +2,10 @@ import math -from spike import PrimeHub, Motor, MotorPair, ColorSensor, MotionSensor, DistanceSensor +from spike import PrimeHub, Motor, MotorPair, ColorSensor, MotionSensor from spike.control import wait_for_seconds -print("Lade IQ-Bibliothek") - +HELLO = "HELLO IQ" ''' Wir nutzen "Duck typing", dh wir schreiben hinter jede Variabel mit ':' die Klasse, zB `leftMotor: Motor` @@ -27,10 +26,6 @@ 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) @@ -45,7 +40,6 @@ class IQRobot: self.bewegungsSensor: MotionSensor = MotionSensor() - self.abstandsSensor: DistanceSensor = DistanceSensor("D") def show(self, image: str): ''' @@ -54,99 +48,65 @@ 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, speed=10): + 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() # Gierwinkel zurücksetzen - + self.bewegungsSensor.reset_yaw_angle() #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 + toleranz = 0 + aktuell = self.bewegungsSensor.get_yaw_angle() ziel = grad steering = 100 if ziel > aktuell else -100 - self.antrieb.start(steering=steering, speed=speed) # Mit bestimmer Geschwindigkeit starten + 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 - 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.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 # 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? + 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() 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 + 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() # Stoppen - self.drehe(-versatz) # Da Versatz immer != 0, korrigieren + self.antrieb.stop() + self.drehe(-versatz) + def fahre_mit_drehung(self, strecke1, grad, strecke2): """ @@ -157,14 +117,11 @@ 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 @@ -183,46 +140,17 @@ class IQRobot: self.antrieb.move(amount=richtung * 0.1, steering=100) drehung = self.bewegungsSensor.get_yaw_angle() print(drehung) - + def heber(self, cm,speed): - """ - Lässt den Heber fahren + self.bothFrontMotors.move_tank(-cm*3.3,"cm", -speed, speed) - :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? - """ + def schaufel(self,prozent): volle_umdrehung=0.29 rotations=volle_umdrehung*prozent/100 - #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: - abstand_gerade = self.abstandsSensor.get_distance_cm() - print(str(abstand_gerade)) - self.antrieb.stop() + self.bothFrontMotors.move(rotations, unit='rotations',speed=20) -print("Fertig geladen.") - +print("successfully loaded the IQ Lego teams code :)") diff --git a/main.py b/main.py index 723c52b..cffdfc7 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,6 +52,14 @@ 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 #################################### ''' @@ -71,15 +79,7 @@ iqRobot: iq.IQRobot = iq.IQRobot(hub) # Führe Funktionen aus unser Robot Klasse aus: iqRobot.show('HAPPY') -def huenchenaufgabe(): - iqRobot.fahre_gerade_aus(40,60) - iqRobot.drehe(-40,True) - iqRobot.fahre_gerade_aus(20,60) - iqRobot.drehe(-20) - iqRobot.fahre_gerade_aus(55,60) - iqRobot.heber(10,30) - -def hologram_alt(): +def hologram_aufgabe1(): iqRobot.fahre_gerade_aus(cm=75,speed=80) iqRobot.drehe(45, False) iqRobot.fahre_gerade_aus(cm=14,speed=70) @@ -87,33 +87,4 @@ def hologram_alt(): iqRobot.drehe(-45, False) iqRobot.fahre_gerade_aus(cm=-75,speed=50) -def druckmaschine(): - iqRobot.fahre_gerade_aus(19,30) - iqRobot.drehe(-45) - iqRobot.fahre_gerade_aus(20,30) - iqRobot.fahre_gerade_aus(-15,30) - -def hologram(): - iqRobot.drehe(45) - iqRobot.fahre_gerade_aus(37.5,30) - iqRobot.drehe(45) - iqRobot.fahre_gerade_aus(15,30) - iqRobot.fahre_gerade_aus(-15,30) - -def augmented_reality(): - iqRobot.drehe(-135) - iqRobot.fahre_gerade_aus(42,30) - iqRobot.drehe(90) - iqRobot.fahre_gerade_aus(12,30) - iqRobot.schaufel(-100) - iqRobot.fahre_gerade_aus(-3,30) - iqRobot.drehe(90) - iqRobot.fahre_gerade_aus(20,30) - iqRobot.drehe(-90) - iqRobot.fahre_gerade_aus(5,20) - -#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 +hologram_aufgabe1()