iqrobot.py

@@ -1,14 +1,9 @@
 # LEGO type:standard slot:6 autostart
 
-import math
-
-from spike import PrimeHub, Motor, MotorPair, ColorSensor, MotionSensor
+from spike import PrimeHub, Motor, MotorPair, ColorSensor
 from spike.control import wait_for_seconds
 
 HELLO = "HELLO IQ"
-BRICKIES_BOT = "brickies"
-BRICKIES_BOT_2 = "brickies_2"
-BACKSTEIN_BOT = "backstein"
 
 '''
 Wir nutzen "Duck typing", dh wir schreiben hinter jede Variabel mit ':' die Klasse, zB `leftMotor: Motor`
@@ -16,53 +11,12 @@ damit man dann später auch wieder Code Completion hat bei Nutzung der Variablen
 '''
 class IQRobot:
 
-    def __init__(self, hub: PrimeHub, typ: str):
-        
+    def __init__(self, hub: PrimeHub, leftMotorPort: str, rightMotorPort: str, colorSensorPort: str):
         self.hub: PrimeHub = hub
-        self.typ=typ
-
-        if self.typ==BACKSTEIN_BOT:
-            # Radantrieb
-            LEFT_MOTOR_PORT = 'F'
-            RIGHT_MOTOR_PORT = 'B'
-            # Motoren für Aufsätze
-            FRONT_MOTOR_RIGHT_PORT = "E"
-            self.frontMotorRight: Motor = Motor(FRONT_MOTOR_RIGHT_PORT)
-
-        elif self.typ==BRICKIES_BOT:
-            # Radantrieb
-            LEFT_MOTOR_PORT = 'E'
-            RIGHT_MOTOR_PORT = 'F'
-            # Motoren für Aufsätze
-            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)
-            # Radius der Antriebsräder
-            self.rad_radius = 2.1
-            # Abstand zwischen Rädern (Mitte) und Vorderseite des Roboters
-            self.abstand_rad_front = 5.55
-            
-        elif self.typ==BRICKIES_BOT_2:    
-            # Radantrieb
-            LEFT_MOTOR_PORT = 'E'
-            RIGHT_MOTOR_PORT = 'F'
-            # Radius der Antriebsräder
-            self.rad_radius = 2.9
-            # Abstand zwischen Rädern (Mitte) und Vorderseite des Roboters
-            self.abstand_rad_front = 8.5
-            
-        ## Allgemein ##        
-        self.movementMotors: MotorPair = MotorPair(LEFT_MOTOR_PORT, RIGHT_MOTOR_PORT)
-        # Radumfang neu berechnen und Motor konfigurieren
-        rad_umfang = 2 * math.pi * self.rad_radius
-        self.movementMotors.set_motor_rotation(rad_umfang)
-        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()
+        self.leftMotor: Motor = Motor(leftMotorPort)
+        self.rightMotor: Motor = Motor(rightMotorPort)
+        self.movementMotors: MotorPair = MotorPair(leftMotorPort, rightMotorPort) 
+        self.colorSensor: ColorSensor = ColorSensor(colorSensorPort)
 
 
     def show(self, image: str):
@@ -79,91 +33,15 @@ class IQRobot:
         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 grad == 0 or grad == 360 :
-            print("nichts zu tun")
-            return
-        if with_reset:
-            self.motionSensor.reset_yaw_angle()
-        #steering = 100 if grad > 0 else -100
-        toleranz = 0
-        aktuell = self.motionSensor.get_yaw_angle()
-        ziel = grad
-        steering = 100 if ziel > aktuell else -100
-        self.movementMotors.start(steering=steering, speed=10)
-        differenz = ziel - aktuell
-        print ("Start Ziel: {}, Aktuell: {}".format(ziel, aktuell))
-        while abs(differenz) > toleranz :
-            aktuell = self.motionSensor.get_yaw_angle()
-            differenz = ziel - aktuell
-            pass
-        self.movementMotors.stop()
-        print ("Final Ziel: {}, Aktuell: {}".format(ziel, aktuell))
 
 
-    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)
-    
-    def fahre_gerade_aus(self, cm,speed):
-    
-        self.motionSensor.reset_yaw_angle()
-    
-        self.movementMotors.move_tank(amount=cm,left_speed=speed, right_speed=speed)
-        drehung = self.motionSensor.get_yaw_angle()
-        print(drehung)
-        if drehung > 0:
-            richtung = -1
-        else:
-            richtung = 1
-        while abs(drehung) > 2:
-            self.movementMotors.move(amount=richtung * 0.1, steering=100)
-            drehung = self.motionSensor.get_yaw_angle()
-            print(drehung)
-
 
 print("successfully loaded the IQ Lego teams code :)")
 
 
-
-
-
-
-
-

iqrobot_schreiber.py

@@ -1,187 +0,0 @@
-# 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

@@ -70,16 +70,22 @@ und auch `hub` als Instanz von PrimeHub
 dh auch an die Funktionen im importierten Code übergeben werde
 '''
 
+# Definiere an welchen Ports die Sensoren angeschlossen sind 
+COLOR_SENSOR_PORT = 'E'
+LEFT_MOTOR_PORT = 'A'
+RIGHT_MOTOR_PORT = 'B'
+
 # Initialisieren des Hubs, der Aktoren und Sensoren
 hub = PrimeHub()
 
 # Initialisiere Robot Klasse mit unseren Funktionen
-iqRobot: iq.IQRobot = iq.IQRobot(hub, typ=iq.BRICKIES_BOT)
+iqRobot: iq.IQRobot = iq.IQRobot(hub, LEFT_MOTOR_PORT, RIGHT_MOTOR_PORT, COLOR_SENSOR_PORT)
 
 # Führe Funktionen aus unser Robot Klasse aus:
 iqRobot.show('HAPPY')
-iqRobot.fahre_mit_drehung(strecke1=10, grad=90, strecke2=10)
-iqRobot.fahre_mit_drehung(strecke1=0, grad=-90, strecke2=10)
+iqRobot.driveForward_for_sec(2.0)
+colorIntensity = iqRobot.getColorIntensity()
+print("Farbintensität: " + str(colorIntensity))