From aa4365c087db1770853df156fc3a7d30145ea672 Mon Sep 17 00:00:00 2001 From: viernst Date: Wed, 17 May 2023 18:08:40 +0200 Subject: [PATCH] Challenge: draw LEGO First commit --- iqrobot.py | 68 ++++++++++++++++++++++++++++++++++++++++++++++-- main.py | 76 ++++++++++++++++++++++++++++++++++++------------------ 2 files changed, 117 insertions(+), 27 deletions(-) diff --git a/iqrobot.py b/iqrobot.py index 200abd9..2d67cd8 100644 --- a/iqrobot.py +++ b/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 \ No newline at end of file + + + +print("Programm Library loaded") \ No newline at end of file diff --git a/main.py b/main.py index ab0bd36..08e2983 100644 --- a/main.py +++ b/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 = '' -LEFT_MOTOR_PORT = '' -RIGHT_MOTOR_PORT = '' +COLOR_SENSOR_PORT = 'D' +LEFT_MOTOR_PORT = 'B' +RIGHT_MOTOR_PORT = 'F' STICK_MOTOR_PORT = '' -GABEL_MOTOR_PORT = '' -DISTANCE_SENSOR_PORT = 'F' - +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, STICK_MOTOR_PORT, GABEL_MOTOR_PORT, DISTANCE_SENSOR_PORT) -''' -while(True):iqRobot.moveGabel(50)or()) - if (iqRobot.colorSensor.get_color() == "cyan"): - iqRobot.driveForward_for_sec(1) - print('Blau du Affe') - elif(iqRobot.colorSensor.get_red() == "red"): - iqRobot.driveForward_for_sec(1) - print('Rot du Affe') - else: - print('Keine Farbe!') -''' +# 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 -while (True): - print('Kurz:' + str(iqRobot.getShortDistance())) - wait_for_seconds(1) +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() - ''' - print('Lang:' + str(iqRobot.getDistance())) - wait_for_seconds(5) - ''' + 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")