Challenge: draw LEGO

First commit

iqrobot.py

@@ -1,9 +1,10 @@
 # LEGO type:standard slot:6 autostart
 
-from spike import PrimeHub, Motor, MotorPair, ColorSensor
+from spike import PrimeHub, Motor, MotorPair, ColorSensor, DistanceSensor, MotionSensor
 from spike.control import wait_for_seconds
 
 HELLO = "HELLO IQ"
+VERSION = "V1"
 
 '''
 Wir nutzen "Duck typing", dh wir schreiben hinter jede Variabel mit ':' die Klasse, zB `leftMotor: Motor`
@@ -11,12 +12,24 @@ 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, leftMotorPort: str, rightMotorPort: str, colorSensorPort: str, stickMotorPort: str, gabelMotorPort: str, distanceSensorPort: str):
         self.hub: PrimeHub = hub
+        if (leftMotorPort != ''):
             self.leftMotor: Motor = Motor(leftMotorPort)
+        if (rightMotorPort != ''):
             self.rightMotor: Motor = Motor(rightMotorPort)
+        if (leftMotorPort != '' and rightMotorPort != ''):
             self.movementMotors: MotorPair = MotorPair(leftMotorPort, rightMotorPort) 
+        if (colorSensorPort != ''):
             self.colorSensor: ColorSensor = ColorSensor(colorSensorPort)
+        if (stickMotorPort != ''):
+            self.stickMotor: Motor = Motor(stickMotorPort)
+        if (gabelMotorPort != ''):
+            self.gabelMotor: Motor = Motor(gabelMotorPort)
+        if (distanceSensorPort != ''):
+            self.distanceSensor: DistanceSensor = DistanceSensor(distanceSensorPort)
+
+        self.motionSensor = MotionSensor()
 
 
     def show(self, image: str):
@@ -33,15 +46,123 @@ class IQRobot:
         wait_for_seconds(seconds)
         self.movementMotors.stop()
 
+            
+    def driveBackward_for_sec(self, seconds: float):
+        # Fahre die übergebene Anzahl seconds gerade aus
+        self.movementMotors.set_default_speed(-100)()
+        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 main(self):
+        
+        '''
+        self.moveStick(30)
+        if self.colorSensor.get_reflected_light() > 1:
+            self.show('ANGRY')
+        else:
+            self.show('SAD')
+
+
+        colorIntensity = self.getColorIntensity()
+        print("Farbintensität: " + str(colorIntensity))
+        '''
+
+    def rotateRobot(self,degrees,speed) :
+        #Lasse den Roboter bei Bedarf um eine bestimmte Anzahl an Grad sanft und behutsam nach Links oder Rechts drehen,
+        #  um den Nutzer zu befriedigen
+        self.motionSensor.reset_yaw_angle()
+        if (degrees > 0):
+            self.leftMotor.start(speed)
+            self.rightMotor.start(-speed)
+        elif (degrees < 0):
+            self.leftMotor.start(-speed)
+            self.rightMotor.start(speed)
+        while (self.motionSensor.get_yaw_angle() > degrees) :
+            self.leftMotor.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
 
 
 
-print("successfully loaded the IQ Lego teams code :)")
+        #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
+        self.stickMotor.run_for_degrees(degrees)
+
+    def moveGabel(self,degrees) :
+        #Bewege sanft und langsam die ungewöhnlich große sich bewegende Stange am vorderen Rumpf des Geräts
+        self.gabelMotor.run_for_degrees(degrees)
+
+    def getDistance (self) :
+        cmDistance = self.distanceSensor.get_distance_cm(short_range=False)
+        return cmDistance
+    
+    def getShortDistance (self) :
+        cmDistance = self.distanceSensor.get_distance_cm(short_range=True)
+        return cmDistance
     
 
+
+
+print("Programm Library loaded")

main.py

@@ -12,6 +12,7 @@ if battery.voltage() < 8000: #set threshold for battery level
     print("Spannung der Batterie zu niedrig: " + str(battery.voltage()) + " \n"
         + "--------------------------------------- \n "
         + "#### UNBEDINGT ROBOTER AUFLADEN !!! #### \n"
+        + "#### SONST KOMMT MARKUS OPTITZ !!!! #### \n"
         + "---------------------------------------- \n")
 else:
     print("Spannung der Batterie " + str(battery.voltage()) + "\n")
@@ -50,44 +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) 
 
-# Importiere Go Robot Code
-#importFile(slotid=3, precompiled=True, module_name="gorobot")
-#import gorobot as gr
-#gr.exampleFour()
-#gr.db.gyroRotation(90, 25, 35, 25)
-
-
-################### Hauptcode ####################################
-'''
-Code zum Lösen einer Aufgabe, kann oben importierten Code nutzen
-Es können auch pro Aufgabe eigene Funktionen geschrieben werden
-Wichtig ist, dass die PORTS der Sensoren überall gleich sind
-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'
 
+COLOR_SENSOR_PORT = 'D'
+LEFT_MOTOR_PORT = 'B'
+RIGHT_MOTOR_PORT = 'F'
+STICK_MOTOR_PORT = ''
+GABEL_MOTOR_PORT = 'E'
+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)
+iqRobot: iq.IQRobot = iq.IQRobot(hub, LEFT_MOTOR_PORT, RIGHT_MOTOR_PORT, COLOR_SENSOR_PORT, STICK_MOTOR_PORT, GABEL_MOTOR_PORT, DISTANCE_SENSOR_PORT)
 
-# 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))
+# Das Programm
  
+# Variablen
+penStatus = False # Oben
 
+# Methoden
+def togglePen():
+    global penStatus
+    if(penStatus == True):
+        iqRobot.moveGabel(20)
+        penStatus = False
+    if(penStatus == False):
+       iqRobot.moveGabel(-20)
+       penStatus = True
 
+def printChar(c):
+    global penStatus
+    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("Printing the character G")
+    if(c == "O"):
+        print("Printing the character O")
 
+wait_for_seconds(5)
+
+printChar("L")
+printChar("E")
+printChar("G")
+printChar("O")