schreibe L mit Backstein Robot

iqrobot.py

@@ -1,9 +1,9 @@
-# LEGO type:standard slot:6 autostart
+# LEGO type:standard slot:7 autostart
 
-from spike import PrimeHub, Motor, MotorPair, ColorSensor
+from spike import PrimeHub, Motor, MotorPair, ColorSensor, MotionSensor
 from spike.control import wait_for_seconds
 
-HELLO = "HELLO IQ"
+HELLO = "HELLO IQ 2"
 
 '''
 Wir nutzen "Duck typing", dh wir schreiben hinter jede Variabel mit ':' die Klasse, zB `leftMotor: Motor`
@@ -16,7 +16,10 @@ class IQRobot:
         self.leftMotor: Motor = Motor(leftMotorPort)
         self.rightMotor: Motor = Motor(rightMotorPort)
         self.movementMotors: MotorPair = MotorPair(leftMotorPort, rightMotorPort) 
-        self.colorSensor: ColorSensor = ColorSensor(colorSensorPort)
+        #self.colorSensor: ColorSensor = ColorSensor(colorSensorPort)
+        #self.frontMotorLeft: Motor = Motor("C")
+        self.frontMotorRight: Motor = Motor("E")
+        self.motionSensor: MotionSensor = MotionSensor()
 
 
     def show(self, image: str):
@@ -38,10 +41,75 @@ class IQRobot:
         (red, green, blue, colorIntensity) = self.colorSensor.get_rgb_intensity()
         return colorIntensity
 
+    def drehe(self, grad=90):
+        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 schreibeL(self, schreibe=True, zurueck=False):
+        if zurueck: 
+            step = 5
+            faktor = -1
+        else:
+            step = 1
+            faktor = 1
+        print("Schreibe L")
+        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 schreibeE(self):
+        print("Schreibe E")
+
+    def schreibeG(self):
+        print("Schreibe G")
+
+
+    def schreibeO(self):
+        print("Schreibe O")
+
+    def schreibeLego(self):
+        self.schreibeL()
+        self.schreibeL(schreibe=False, zurueck=True)
+        self.schreibeE()
+        self.schreibeG()
+        self.schreibeO()
+
 print("successfully loaded the IQ Lego teams code :)")
 
 

main.py

@@ -1,4 +1,4 @@
-# LEGO type:standard slot:5 autostart
+# LEGO type:standard slot:4 autostart
 
 import os, sys
 
@@ -33,9 +33,10 @@ def importFile(slotid=0, precompiled=False, module_name='importFile'):
         program = f.read()
         print(program)
     try:
-        os.remove("/"+module_name+suffix)
+        os.remove("/"+module_name+".py")
+        os.remove("/"+module_name+".mpy")
     except:
-        pass
+        print("Couldn't remove old module")
     with open("/"+module_name+suffix,"w+") as f:
         print("trying to write import program")
         f.write(program)
@@ -50,7 +51,7 @@ def importFile(slotid=0, precompiled=False, module_name='importFile'):
 
 # Importiere Code aus der Datei "iqrobot.py"
 # Dateiname und Modulname sollten gleich sein, dann kann man Code Completion nutzen
-importFile(slotid=6, precompiled=True, module_name="iqrobot")
+importFile(slotid=7, precompiled=True, module_name="iqrobot")
 import iqrobot as iq
 print(iq.HELLO) 
 
@@ -71,8 +72,8 @@ 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'
+COLOR_SENSOR_PORT = 'E' #not implemented yet
+LEFT_MOTOR_PORT = 'F'
 RIGHT_MOTOR_PORT = 'B'
 
 # Initialisieren des Hubs, der Aktoren und Sensoren
@@ -83,10 +84,7 @@ 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.schreibeLego()