iqrobot.py

@@ -1,6 +1,8 @@
 # LEGO type:standard slot:6 autostart
 
-from spike import PrimeHub, Motor, MotorPair, ColorSensor
+import math
+
+from spike import PrimeHub, Motor, MotorPair, ColorSensor, MotionSensor
 from spike.control import wait_for_seconds
 
 HELLO = "HELLO IQ"
@@ -13,10 +15,35 @@ class IQRobot:
 
     def __init__(self, hub: PrimeHub, leftMotorPort: str, rightMotorPort: str, colorSensorPort: str):
         self.hub: PrimeHub = hub
-        self.leftMotor: Motor = Motor(leftMotorPort)
-        self.rightMotor: Motor = Motor(rightMotorPort)
-        self.movementMotors: MotorPair = MotorPair(leftMotorPort, rightMotorPort) 
-        self.colorSensor: ColorSensor = ColorSensor(colorSensorPort)
+        self.typ=typ
+        if self.typ=="backstein":
+            LEFT_MOTOR_PORT = 'F'
+            RIGHT_MOTOR_PORT = 'B'
+            FRONT_MOTOR_RIGHT_PORT = "E"
+            self.frontMotorRight: Motor = Motor(FRONT_MOTOR_RIGHT_PORT)
+            self.movementMotors: MotorPair = MotorPair(LEFT_MOTOR_PORT, RIGHT_MOTOR_PORT)
+        elif self.typ=="brickies":
+            LEFT_MOTOR_PORT = 'E'
+            RIGHT_MOTOR_PORT = 'F'
+            FRONT_MOTOR_RIGHT_PORT = "B"
+            FRONT_MOTOR_LEFT_PORT = "A"
+            self.frontMotorRight: Motor = Motor(FRONT_MOTOR_RIGHT_PORT)
+            self.frontMotorLeft: Motor = Motor(FRONT_MOTOR_LEFT_PORT)
+            self.bothFrontMotors: MotorPair = MotorPair(FRONT_MOTOR_LEFT_PORT, FRONT_MOTOR_RIGHT_PORT)
+            self.movementMotors: MotorPair = MotorPair(LEFT_MOTOR_PORT, RIGHT_MOTOR_PORT)
+            # Radumfang neu berechnen und Motor konfigurieren
+            rad_radius = 2.1
+            rad_umfang = 2 * math.pi * rad_radius
+            self.movementMotors.set_motor_rotation(rad_umfang)
+            # Abstand zwischen Rädern (Mitte) und Vorderseite des Roboters
+            self.abstand_rad_front = 5.55
+
+        self.leftMotor: Motor = Motor(LEFT_MOTOR_PORT)
+        self.rightMotor: Motor = Motor(RIGHT_MOTOR_PORT)
+         
+        #self.colorSensor: ColorSensor = ColorSensor(colorSensorPort)
+        #self.frontMotorLeft: Motor = Motor("C")
+        self.motionSensor: MotionSensor = MotionSensor()
 
 
     def show(self, image: str):
@@ -35,12 +62,43 @@ class IQRobot:
 
     def getColorIntensity(self):
         # Ermittele Farbintensität über den Farbsensor
-        (red, green, blue, colorIntensity) = self.colorSensor.get_rgb_intensity()
+        (red, green, blue, colorIntensity) = self   .colorSensor.get_rgb_intensity()
         return colorIntensity
 
 
+    def fahre_gerade(self, cm):
+        if self.typ == "brickies":
+            cm = -cm
+        self.motionSensor.reset_yaw_angle()
+        self.movementMotors.start_tank(10, 10)
+        self.movementMotors.set_default_speed(10)
+        self.movementMotors.move(cm)
+        versatz = self.motionSensor.get_yaw_angle()
+        self.drehe(grad=-versatz)
     
 
+    def fahre_gerade_geregelt(self, cm):
+        if self.typ == "brickies":
+            cm = -cm
+        self.motionSensor.reset_yaw_angle()
+        self.movementMotors.start_tank(10, 10)
+        self.movementMotors.set_default_speed(10)
+
+        sum_cm = 0
+        while sum_cm < cm:
+            self.movementMotors.move(1)
+            versatz = self.motionSensor.get_yaw_angle()
+            self.drehe(grad=-versatz)
+            self.motionSensor.reset_yaw_angle()
+            sum_cm = sum_cm + 1
+
+        self.movementMotors.move(cm - sum_cm)
+
+    def fahre_mit_drehung(self, strecke1, grad, strecke2):
+        self.fahre_gerade_geregelt(strecke1 + self.abstand_rad_front)
+        self.drehe(grad)
+        self.fahre_gerade_geregelt(strecke2 - self.abstand_rad_front)
+
 
 print("successfully loaded the IQ Lego teams code :)")
 

iqrobot_schreiber.py

@@ -0,0 +1,187 @@
+# LEGO type:standard slot:7 autostart
+
+########################################################################
+# "ALTE" VERSION MIT DER WIR VERSUCHT HABEN DAS WORT "LEGO" ZU SCHREIBEN
+########################################################################
+
+from spike import PrimeHub, Motor, MotorPair, ColorSensor, MotionSensor
+from spike.control import wait_for_seconds
+
+HELLO = "HELLO IQ 2"
+
+'''
+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, colorSensorPort: str, typ: str):
+        self.hub: PrimeHub = hub
+        self.typ=typ
+        if self.typ=="backstein":
+            LEFT_MOTOR_PORT = 'F'
+            RIGHT_MOTOR_PORT = 'B'
+            FRONT_MOTOR_RIGHT_PORT = "E"
+            self.frontMotorRight: Motor = Motor(FRONT_MOTOR_RIGHT_PORT)
+        elif self.typ=="brickies":
+            LEFT_MOTOR_PORT = 'E'
+            RIGHT_MOTOR_PORT = 'F'
+            FRONT_MOTOR_RIGHT_PORT = "B"
+            FRONT_MOTOR_LEFT_PORT = "A"
+            self.frontMotorRight: Motor = Motor(FRONT_MOTOR_RIGHT_PORT)
+            self.frontMotorLeft: Motor = Motor(FRONT_MOTOR_LEFT_PORT)
+            self.bothFrontMotors: MotorPair = MotorPair(FRONT_MOTOR_LEFT_PORT, FRONT_MOTOR_RIGHT_PORT)
+
+        self.leftMotor: Motor = Motor(LEFT_MOTOR_PORT)
+        self.rightMotor: Motor = Motor(RIGHT_MOTOR_PORT)
+        self.movementMotors: MotorPair = MotorPair(LEFT_MOTOR_PORT, RIGHT_MOTOR_PORT) 
+        #self.colorSensor: ColorSensor = ColorSensor(colorSensorPort)
+        #self.frontMotorLeft: Motor = Motor("C")
+        self.motionSensor: MotionSensor = MotionSensor()
+
+
+    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 driveForward_for_sec(self, seconds: float):
+        # Fahre die übergebene Anzahl seconds gerade aus
+        self.movementMotors.start()
+        wait_for_seconds(seconds)
+        self.movementMotors.stop()
+
+    def getColorIntensity(self):
+        # Ermittele Farbintensität über den Farbsensor
+        (red, green, blue, colorIntensity) = self.colorSensor.get_rgb_intensity()
+        return colorIntensity
+
+    def drehe(self, grad=90, with_reset=True):
+        if with_reset:
+            self.motionSensor.reset_yaw_angle()
+        steering = 100 if grad > 0 else -100
+        self.movementMotors.start(steering=steering, speed=10)
+        while abs(self.motionSensor.get_yaw_angle()) < abs(grad):
+            pass
+        self.movementMotors.stop()
+
+
+    def drehe_robot(self, grad=90):
+        if self.typ == "backstein":
+            radius=9.5
+            stift_versatz=2.2
+        if self.typ == "brickies":
+            radius=17.4
+            stift_versatz=0.3
+        self.fahre_gerade(-radius - stift_versatz)
+        self.drehe(grad)
+        self.fahre_gerade(radius - stift_versatz)
+
+    def fahre_gerade(self, cm, zeichne=False):
+        if zeichne:
+            self.bewege_stift(1) # Stift runter
+        self.motionSensor.reset_yaw_angle()
+        if self.typ == "brickies":
+            cm = -cm
+        self.movementMotors.move(cm)
+        if zeichne:
+            self.bewege_stift(-1) # Stift hoch
+        versatz = self.motionSensor.get_yaw_angle()
+        self.drehe(grad=-versatz)
+
+    def buchstabe_zu_segmenten(self, buchstabe):
+        # Segmente um Buchstaben zu schreiben
+        #          4_
+        #       5 |__|3
+        #       0 |6_|2
+        #          1
+        #
+        buchstabe_zu_segmenten = {"L": [1,1,0,0,0,1,0], "E": [1,1,0,0,1,1,1], "G": [1,1,1,0,1,1,0], "O": [1,1,1,1,1,1,0]}
+        return buchstabe_zu_segmenten[buchstabe]
+
+
+    def bewege_stift(self, richtung):
+        if self.typ == "backstein":
+            self.frontMotorRight.run_for_rotations(richtung*0.4)
+        if self.typ == "brickies":
+            #print("bewege stift brickies")
+            self.bothFrontMotors.move(-richtung*0.2, unit='rotations', speed=5)
+
+    def schreibe_buchstabe(self, buchstabe):
+        print("Schreibe " + buchstabe)
+        segmente = self.buchstabe_zu_segmenten(buchstabe)
+        grad_drehung=-90
+        self.fahre_gerade(2)
+        self.drehe_robot(-grad_drehung) # drehe rechts
+        for segment_nummer, segment in enumerate(segmente):
+            print("Segment: " + str(segment) + " , Segment Nummer: " + str(segment_nummer))
+            if segment==1:
+                self.fahre_gerade(5, zeichne=True)
+            else:
+                self.fahre_gerade(5)
+            if segment_nummer != 2 and segment_nummer != 6:
+                self.drehe_robot(grad_drehung) # drehe links
+            
+
+    def schreibeL(self, schreibe=True, zurueck=False):
+        if zurueck: 
+            step = 5
+            faktor = -1
+        else:
+            step = 1
+            faktor = 1
+        print("Schreibe L")
+        #self.frontMotorRight.run_for_rotations(-0.4)
+        radius=9.5
+        stift_versatz=2.2
+        if schreibe:
+            self.frontMotorRight.run_for_rotations(0.4)
+        self.movementMotors.set_default_speed(10)
+
+        while (True):
+            if step == 0:
+                break
+            if step == 1:
+                self.movementMotors.move(faktor * 5)
+                if schreibe:
+                    self.frontMotorRight.run_for_rotations(-0.4)
+            if step == 2:
+                self.movementMotors.move(faktor * (-radius - stift_versatz))
+            if step == 3:
+                self.drehe(faktor * -90)
+            if step == 4:
+                self.movementMotors.move(faktor*(radius - stift_versatz))
+                if schreibe:
+                    self.frontMotorRight.run_for_rotations(0.4)
+            if step == 5:
+                self.movementMotors.move(faktor * 2)
+                if schreibe:
+                    self.frontMotorRight.run_for_rotations(-0.4)
+            if step == 6:
+                break
+            step += faktor
+        # Fahre 5 cm rückwerts  
+        # dann drehe nach rechts 90°
+        # und fahre 2cm fohrwärts 
+        #stift hoch
+
+
+    def schreibeLego(self):
+        #self.schreibeL()
+        #self.schreibeL(schreibe=False, zurueck=True)
+        self.movementMotors.set_default_speed(10)
+        self.bewege_stift(-1)
+        self.fahre_gerade(4, zeichne=True)
+        self.drehe_robot()
+        self.fahre_gerade(4, zeichne=True)
+        #self.schreibe_buchstabe("L")
+        #self.schreibe_buchstabe("E")
+        #self.schreibe_buchstabe("G")
+        #self.schreibe_buchstabe("O")
+
+print("successfully loaded the IQ Lego teams code :)")
+
+

main.py

@@ -83,9 +83,8 @@ iqRobot: iq.IQRobot = iq.IQRobot(hub, LEFT_MOTOR_PORT, RIGHT_MOTOR_PORT, COLOR_S
 
 # Führe Funktionen aus unser Robot Klasse aus:
 iqRobot.show('HAPPY')
-iqRobot.driveForward_for_sec(2.0)
-colorIntensity = iqRobot.getColorIntensity()
-print("Farbintensität: " + str(colorIntensity))
+iqRobot.fahre_mit_drehung(strecke1=10, grad=90, strecke2=10)
+iqRobot.fahre_mit_drehung(strecke1=0, grad=-90, strecke2=10)