Challenge: draw LEGO

First commit

iqrobot.py

@@ -29,6 +29,9 @@ class IQRobot:
         if (distanceSensorPort != ''):
             self.distanceSensor: DistanceSensor = DistanceSensor(distanceSensorPort)
 
+        self.motionSensor = MotionSensor()
+
+
     def show(self, image: str):
         '''
         Zeige Bild auf LED Matrix des Spikes
@@ -83,7 +86,65 @@ class IQRobot:
             self.rightMotor.start(speed)
         while (self.motionSensor.get_yaw_angle() > degrees) :
             self.leftMotor.stop()
-            self.rightMotor.stop()   
+            self.rightMotor.stop()
+        print("Disabled")  
+
+    def gyroRotation(self, angle, rotate_mode = 0, stop = True):
+
+        global run_generator, runSmall
+
+        run_generator = False
+
+        speed = 30
+
+        #set standard variables
+        rotatedDistance = 0
+        steering = 1
+
+        #gyro sensor calibration
+        angle = angle * (2400/2443) #experimental value based on 20 rotations of the robot
+
+        #Setting variables based on inputs
+        loop = True
+        gyroStartValue = self.motionSensor.get_yaw_angle() #Yaw angle used due to orientation of the self.hub. This might need to be changed
+
+        #Inversion of steering value for turning counter clockwise
+        if angle < 0:
+            steering = -1
+
+        #Testing to see if turining is necessary, turns until loop = False
+
+        while loop:
+            rotatedDistance = self.motionSensor.get_yaw_angle() #Yaw angle used due to orientation of the self.hub. This might need to be changed
+            
+            print(abs(rotatedDistance - gyroStartValue))
+
+            #Checking for variants
+            #Both Motors turn, robot moves on the spot
+            if rotate_mode == 0:
+                self.rightMotor.start_at_power(int(speed) * steering)
+                self.leftMotor.start_at_power(int(speed) * steering)
+            
+            elif rotate_mode == 1:
+
+                if angle * speed > 0:
+                    self.leftMotor.start_at_power(- int(speed))
+                else:
+                    self.rightMotor.start_at_power(+ int(speed))
+
+            
+            if abs(angle) <= abs(rotatedDistance - gyroStartValue):                   
+                    loop = False
+                    break
+
+
+
+        #Stops movement motors for increased accuracy while stopping
+        if stop:
+            self.rightMotor.stop()
+            self.leftMotor.stop()
+
+        return # End of gyroStraightDrive 
             
     def moveStick(self,degrees) :
         #Bewege sanft und langsam die Schleuderstange am hinteren Rumpf des Geräts
@@ -101,4 +162,7 @@ class IQRobot:
         cmDistance = self.distanceSensor.get_distance_cm(short_range=True)
         return cmDistance
     
-    print("Loading library IQRobot in version " + VERSION)from spike import PrimeHub, Motor, MotorPair, ColorSensor, MotionSensor
+
+
+
+print("Programm Library loaded")

main.py

@@ -51,43 +51,69 @@ 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=6, precompiled=False, module_name="iqrobot")
 import iqrobot as iq
 print(iq.HELLO) 
 
 # Definiere an welchen Ports die Sensoren angeschlossen sind 
 
-COLOR_SENSOR_PORT = ''
+COLOR_SENSOR_PORT = 'D'
-LEFT_MOTOR_PORT = ''
+LEFT_MOTOR_PORT = 'B'
-RIGHT_MOTOR_PORT = ''
+RIGHT_MOTOR_PORT = 'F'
 STICK_MOTOR_PORT = ''
-GABEL_MOTOR_PORT = ''
+GABEL_MOTOR_PORT = 'E'
-DISTANCE_SENSOR_PORT = 'F'
+DISTANCE_SENSOR_PORT = ''
-
+#penStatus = False
 # 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, STICK_MOTOR_PORT, GABEL_MOTOR_PORT, DISTANCE_SENSOR_PORT)
 
-'''
+# Das Programm
-while(True):iqRobot.moveGabel(50)or())
+ 
-    if (iqRobot.colorSensor.get_color() == "cyan"):
+# Variablen
-        iqRobot.driveForward_for_sec(1)
+penStatus = False # Oben
-        print('Blau du Affe')
-    elif(iqRobot.colorSensor.get_red() == "red"):
-        iqRobot.driveForward_for_sec(1)
-        print('Rot du Affe')
-    else:
-        print('Keine Farbe!')
-'''
 
+# Methoden
+def togglePen():
+    global penStatus
+    if(penStatus == True):
+        iqRobot.moveGabel(20)
+        penStatus = False
+    if(penStatus == False):
+       iqRobot.moveGabel(-20)
+       penStatus = True
 
-while (True):
+def printChar(c):
-    print('Kurz:' + str(iqRobot.getShortDistance()))
+    global penStatus
-    wait_for_seconds(1)
+    if(c == "L"):
+        print("Printing the character L")
+        if not(penStatus == "False"):
+            print("Toggling pen")
+            togglePen()
+        iqRobot.driveForward_for_sec(2.0)
+        # iqRobot.leftMotor.run_for_degrees(360)
+        # iqRobot.rotateRobot(degrees=100, speed=100)
+        iqRobot.gyroRotation(angle=90)
+        iqRobot.driveForward_for_sec(1.0)
+    if(c == "E"):
+        print("Printing the character E")
+        if(penStatus == True):
+            togglePen()
+        iqRobot.driveForward_for_sec(0.1)
+        togglePen()
+        iqRobot.driveForward_for_sec(1.0)
+        togglePen()
 
-    '''
+    if(c == "G"):
-    print('Lang:' + str(iqRobot.getDistance()))
+        print("Printing the character G")
-    wait_for_seconds(5)
+    if(c == "O"):
-    '''
+        print("Printing the character O")
+
+wait_for_seconds(5)
+
+printChar("L")
+printChar("E")
+printChar("G")
+printChar("O")