iqrobot.py

@@ -6,9 +6,6 @@ from spike import PrimeHub, Motor, MotorPair, ColorSensor, MotionSensor
 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 +13,30 @@ 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):
         
         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)
+        # Radantrieb
+        LEFT_MOTOR_PORT = 'E'
+        RIGHT_MOTOR_PORT = 'F'
 
-        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
+        # Motoren für Aufsätze
+        FRONT_MOTOR_RIGHT_PORT = "B"
+        FRONT_MOTOR_LEFT_PORT = "A"
 
-        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
+        self.bothFrontMotors: MotorPair = MotorPair(FRONT_MOTOR_LEFT_PORT, FRONT_MOTOR_RIGHT_PORT)
 
-        ## Allgemein ##        
-        self.movementMotors: MotorPair = MotorPair(LEFT_MOTOR_PORT, RIGHT_MOTOR_PORT)
+        self.antrieb: MotorPair = MotorPair(LEFT_MOTOR_PORT, RIGHT_MOTOR_PORT)
         # Radumfang neu berechnen und Motor konfigurieren
+        # Radius der Antriebsräder
+        self.rad_radius = 2.1
+        # Abstand zwischen Rädern (Mitte) und Vorderseite des Roboters
+        self.abstand_rad_front = 5.55            
         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.antrieb.set_motor_rotation(rad_umfang)
+
+        self.bewegungsSensor: MotionSensor = MotionSensor()
 
 
     def show(self, image: str):
@@ -73,88 +47,84 @@ class IQRobot:
         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):
+        """
+        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
+        """
         if grad == 0 or grad == 360 :
             print("nichts zu tun")
             return
         if with_reset:
-            self.motionSensor.reset_yaw_angle()
+            self.bewegungsSensor.reset_yaw_angle()
         #steering = 100 if grad > 0 else -100
         toleranz = 0
-        aktuell = self.motionSensor.get_yaw_angle()
+        aktuell = self.bewegungsSensor.get_yaw_angle()
         ziel = grad
         steering = 100 if ziel > aktuell else -100
-        self.movementMotors.start(steering=steering, speed=10)
+        self.antrieb.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()
+            aktuell = self.bewegungsSensor.get_yaw_angle()
             differenz = ziel - aktuell
             pass
-        self.movementMotors.stop()
+        self.antrieb.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)
+        """
+        WIP by Lars & Klaus
+        """
+        self.bewegungsSensor.reset_yaw_angle()
+        self.antrieb.start_tank(10, 10)
+        self.antrieb.set_default_speed(10)
 
         sum_cm = 0
         while sum_cm < cm:
-            self.movementMotors.move(1)
-            versatz = self.motionSensor.get_yaw_angle()
+            self.antrieb.move(1)
+            versatz = self.bewegungsSensor.get_yaw_angle()
             self.drehe(grad=-versatz)
-            self.motionSensor.reset_yaw_angle()
+            self.bewegungsSensor.reset_yaw_angle()
             sum_cm = sum_cm + 1
 
-        self.movementMotors.move(cm - sum_cm)
+        self.antrieb.move(cm - sum_cm)
+
 
     def fahre_mit_drehung(self, strecke1, grad, strecke2):
+        """
+        Funktion für eine Fahrt mit 1. Strecke, dann Drehung in der Mitte, dann 2. Strecke
+        Vereinfacht die Logik, da der Roboter durch die Drehung einen Versatz hat gegenüber einer
+        Strecke die mit dem Lineal ausgemessen wurde
+        """
         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()
+    def fahre_gerade_aus(self, cm: float, speed: int):
+        """
+        Funktion zum gerade aus fahren mit Korrektur am Ende
 
-        self.movementMotors.move_tank(amount=cm,left_speed=speed, right_speed=speed)
-        drehung = self.motionSensor.get_yaw_angle()
+        :param int cm: Zentimeter die gerade aus gefahren werden soll
+        :param speed: Geschwindigkeit mit der gefahren wird
+        """
+    
+        self.bewegungsSensor.reset_yaw_angle()
+    
+        self.antrieb.move_tank(amount=cm,left_speed=speed, right_speed=speed)
+        drehung = self.bewegungsSensor.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()
+            self.antrieb.move(amount=richtung * 0.1, steering=100)
+            drehung = self.bewegungsSensor.get_yaw_angle()
             print(drehung)
 
         

main.py

@@ -74,7 +74,7 @@ dh auch an die Funktionen im importierten Code übergeben werde
 hub = PrimeHub()
 
 # Initialisiere Robot Klasse mit unseren Funktionen
-iqRobot: iq.IQRobot = iq.IQRobot(hub, typ=iq.BRICKIES_BOT_2)
+iqRobot: iq.IQRobot = iq.IQRobot(hub)
 
 # Führe Funktionen aus unser Robot Klasse aus:
 iqRobot.show('HAPPY')