From 0d1a84761a67d8665c6255651d9ba41587c296de Mon Sep 17 00:00:00 2001
From: unknown <legouser@laptop-01.linuxmuster.lan>
Date: Wed, 5 Jul 2023 18:31:48 +0200
Subject: [PATCH] Anpassung auf Brickies Robot

---
 iqrobot.py | 56 ++++++++++++++++++++++++++++++++++++++----------------
 main.py    |  6 +++---
 2 files changed, 43 insertions(+), 19 deletions(-)

diff --git a/iqrobot.py b/iqrobot.py
index 95c6715..f1f02d4 100644
--- a/iqrobot.py
+++ b/iqrobot.py
@@ -11,14 +11,28 @@ damit man dann später auch wieder Code Completion hat bei Nutzung der Variablen
 '''
 class IQRobot:
 
-    def __init__(self, hub: PrimeHub, leftMotorPort: str, rightMotorPort: str, colorSensorPort: str):
+    def __init__(self, hub: PrimeHub, colorSensorPort: str, typ: str):
         self.hub: PrimeHub = hub
-        self.leftMotor: Motor = Motor(leftMotorPort)
-        self.rightMotor: Motor = Motor(rightMotorPort)
-        self.movementMotors: MotorPair = MotorPair(leftMotorPort, rightMotorPort) 
+        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.frontMotorRight: Motor = Motor("E")
         self.motionSensor: MotionSensor = MotionSensor()
 
 
@@ -50,14 +64,20 @@ class IQRobot:
         self.movementMotors.stop()
 
 
-    def drehe_backstein_robot(self, grad=90):
-        radius=9.5
-        stift_versatz=2.2
-        self.movementMotors.move(-radius - stift_versatz)
+    def drehe_robot(self, grad=90):
+        if self.typ == "backstein":
+            radius=9.5
+            stift_versatz=2.2
+        if self.typ == "brickies":
+            radius=11.5
+            stift_versatz=0.5
+        self.fahre_gerade(-radius - stift_versatz)
         self.drehe(grad)
-        self.movementMotors.move(radius - stift_versatz)
+        self.fahre_gerade(radius - stift_versatz)
 
     def fahre_gerade(self, cm):
+        if self.typ == "brickies":
+            cm = -cm
         self.movementMotors.move(cm)
 
     def buchstabe_zu_segmenten(self, buchstabe):
@@ -72,23 +92,26 @@ class IQRobot:
 
 
     def bewege_stift(self, richtung):
-        self.frontMotorRight.run_for_rotations(richtung*0.4)
+        if self.typ == "backstein":
+            self.frontMotorRight.run_for_rotations(richtung*0.4)
+        if self.typ == "brickies":
+            self.bothFrontMotors.move(-richtung*0.2, unit='rotations')
 
     def schreibe_buchstabe(self, buchstabe):
         print("Schreibe " + buchstabe)
         segmente = self.buchstabe_zu_segmenten(buchstabe)
         grad_drehung=-90
         self.fahre_gerade(2)
-        self.drehe_backstein_robot(-grad_drehung) # drehe rechts
-        for segment, segment_nummer in enumerate(segmente):
+        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.bewege_stift(-1)
+                self.bewege_stift(1) # Stift runter
             self.fahre_gerade(5)
             if segment==1:
-                self.bewege_stift(1)
+                self.bewege_stift(-1) # Stift hoch
             if segment_nummer != 2 and segment_nummer != 6:
-                self.drehe_backstein_robot(grad_drehung) # drehe links
+                self.drehe_robot(grad_drehung) # drehe links
             
 
     def schreibeL(self, schreibe=True, zurueck=False):
@@ -137,6 +160,7 @@ class IQRobot:
     def schreibeLego(self):
         #self.schreibeL()
         #self.schreibeL(schreibe=False, zurueck=True)
+        self.movementMotors.set_default_speed(10)
         self.schreibe_buchstabe("L")
         self.schreibe_buchstabe("E")
         self.schreibe_buchstabe("G")
diff --git a/main.py b/main.py
index 2179ecb..8f608a5 100644
--- a/main.py
+++ b/main.py
@@ -73,18 +73,18 @@ dh auch an die Funktionen im importierten Code übergeben werde
 
 # Definiere an welchen Ports die Sensoren angeschlossen sind 
 COLOR_SENSOR_PORT = 'E' #not implemented yet
-LEFT_MOTOR_PORT = 'F'
-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, LEFT_MOTOR_PORT, RIGHT_MOTOR_PORT, COLOR_SENSOR_PORT)
+iqRobot: iq.IQRobot = iq.IQRobot(hub, COLOR_SENSOR_PORT, typ="brickies")
 
 # Führe Funktionen aus unser Robot Klasse aus:
 iqRobot.show('HAPPY')
 iqRobot.schreibeLego()
+#iqRobot.schreibeL()