diff --git a/iqrobot.py b/iqrobot.py index 3aa2ed9..1595e5b 100644 --- a/iqrobot.py +++ b/iqrobot.py @@ -1,14 +1,9 @@ # LEGO type:standard slot:6 autostart -import math - -from spike import PrimeHub, Motor, MotorPair, ColorSensor, MotionSensor +from spike import PrimeHub, Motor, MotorPair, ColorSensor from spike.control import wait_for_seconds HELLO = "HELLO IQ" -BRICKIES_BOT = "brickies" -BRICKIES_BOT_2 = "brickies_2" -BACKSTEIN_BOT = "backstein" ''' Wir nutzen "Duck typing", dh wir schreiben hinter jede Variabel mit ':' die Klasse, zB `leftMotor: Motor` @@ -16,53 +11,12 @@ damit man dann später auch wieder Code Completion hat bei Nutzung der Variablen ''' class IQRobot: - def __init__(self, hub: PrimeHub, typ: str): - + def __init__(self, hub: PrimeHub, leftMotorPort: str, rightMotorPort: str, colorSensorPort: str): self.hub: PrimeHub = hub - self.typ=typ - - if self.typ==BACKSTEIN_BOT: - # Radantrieb - LEFT_MOTOR_PORT = 'F' - RIGHT_MOTOR_PORT = 'B' - # Motoren für Aufsätze - FRONT_MOTOR_RIGHT_PORT = "E" - self.frontMotorRight: Motor = Motor(FRONT_MOTOR_RIGHT_PORT) - - elif self.typ==BRICKIES_BOT: - # Radantrieb - LEFT_MOTOR_PORT = 'E' - RIGHT_MOTOR_PORT = 'F' - # Motoren für Aufsätze - 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) - # Radius der Antriebsräder - self.rad_radius = 2.1 - # Abstand zwischen Rädern (Mitte) und Vorderseite des Roboters - self.abstand_rad_front = 5.55 - - elif self.typ==BRICKIES_BOT_2: - # Radantrieb - LEFT_MOTOR_PORT = 'E' - RIGHT_MOTOR_PORT = 'F' - # Radius der Antriebsräder - self.rad_radius = 2.9 - # Abstand zwischen Rädern (Mitte) und Vorderseite des Roboters - self.abstand_rad_front = 8.5 - - ## Allgemein ## - self.movementMotors: MotorPair = MotorPair(LEFT_MOTOR_PORT, RIGHT_MOTOR_PORT) - # Radumfang neu berechnen und Motor konfigurieren - rad_umfang = 2 * math.pi * self.rad_radius - self.movementMotors.set_motor_rotation(rad_umfang) - self.leftMotor: Motor = Motor(LEFT_MOTOR_PORT) - self.rightMotor: Motor = Motor(RIGHT_MOTOR_PORT) - #self.colorSensor: ColorSensor = ColorSensor(colorSensorPort) - #self.frontMotorLeft: Motor = Motor("C") - self.motionSensor: MotionSensor = MotionSensor() + self.leftMotor: Motor = Motor(leftMotorPort) + self.rightMotor: Motor = Motor(rightMotorPort) + self.movementMotors: MotorPair = MotorPair(leftMotorPort, rightMotorPort) + self.colorSensor: ColorSensor = ColorSensor(colorSensorPort) def show(self, image: str): @@ -79,91 +33,15 @@ class IQRobot: 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() + (red, green, blue, colorIntensity) = self.colorSensor.get_rgb_intensity() return colorIntensity - def drehe(self, grad=90, with_reset=True): - if grad == 0 or grad == 360 : - print("nichts zu tun") - return - if with_reset: - self.motionSensor.reset_yaw_angle() - #steering = 100 if grad > 0 else -100 - toleranz = 0 - aktuell = self.motionSensor.get_yaw_angle() - ziel = grad - steering = 100 if ziel > aktuell else -100 - self.movementMotors.start(steering=steering, speed=10) - differenz = ziel - aktuell - print ("Start Ziel: {}, Aktuell: {}".format(ziel, aktuell)) - while abs(differenz) > toleranz : - aktuell = self.motionSensor.get_yaw_angle() - differenz = ziel - aktuell - pass - self.movementMotors.stop() - print ("Final Ziel: {}, Aktuell: {}".format(ziel, aktuell)) - def fahre_gerade(self, cm): - if self.typ == "brickies": - cm = -cm - self.motionSensor.reset_yaw_angle() - self.movementMotors.start_tank(10, 10) - self.movementMotors.set_default_speed(10) - self.movementMotors.move(cm) - versatz = self.motionSensor.get_yaw_angle() - self.drehe(grad=-versatz) - - def fahre_gerade_geregelt(self, cm): - if self.typ == "brickies": - cm = -cm - self.motionSensor.reset_yaw_angle() - self.movementMotors.start_tank(10, 10) - self.movementMotors.set_default_speed(10) - - sum_cm = 0 - while sum_cm < cm: - self.movementMotors.move(1) - versatz = self.motionSensor.get_yaw_angle() - self.drehe(grad=-versatz) - self.motionSensor.reset_yaw_angle() - sum_cm = sum_cm + 1 - - self.movementMotors.move(cm - sum_cm) - - def fahre_mit_drehung(self, strecke1, grad, strecke2): - self.fahre_gerade_geregelt(strecke1 + self.abstand_rad_front) - self.drehe(grad) - self.fahre_gerade_geregelt(strecke2 - self.abstand_rad_front) - - def fahre_gerade_aus(self, cm,speed): - - self.motionSensor.reset_yaw_angle() - - self.movementMotors.move_tank(amount=cm,left_speed=speed, right_speed=speed) - drehung = self.motionSensor.get_yaw_angle() - print(drehung) - if drehung > 0: - richtung = -1 - else: - richtung = 1 - while abs(drehung) > 2: - self.movementMotors.move(amount=richtung * 0.1, steering=100) - drehung = self.motionSensor.get_yaw_angle() - print(drehung) - - print("successfully loaded the IQ Lego teams code :)") - - - - - - diff --git a/iqrobot_schreiber.py b/iqrobot_schreiber.py deleted file mode 100644 index 404e187..0000000 --- a/iqrobot_schreiber.py +++ /dev/null @@ -1,187 +0,0 @@ -# LEGO type:standard slot:7 autostart - -######################################################################## -# "ALTE" VERSION MIT DER WIR VERSUCHT HABEN DAS WORT "LEGO" ZU SCHREIBEN -######################################################################## - -from spike import PrimeHub, Motor, MotorPair, ColorSensor, MotionSensor -from spike.control import wait_for_seconds - -HELLO = "HELLO IQ 2" - -''' -Wir nutzen "Duck typing", dh wir schreiben hinter jede Variabel mit ':' die Klasse, zB `leftMotor: Motor` -damit man dann später auch wieder Code Completion hat bei Nutzung der Variablen -''' -class IQRobot: - - def __init__(self, hub: PrimeHub, colorSensorPort: str, typ: str): - self.hub: PrimeHub = hub - 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.motionSensor: MotionSensor = MotionSensor() - - - def show(self, image: str): - ''' - Zeige Bild auf LED Matrix des Spikes - image: Bildname wie zB 'HAPPY' - ''' - self.hub.light_matrix.show_image(image) - - - def driveForward_for_sec(self, seconds: float): - # Fahre die übergebene Anzahl seconds gerade aus - 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 drehe(self, grad=90, with_reset=True): - if with_reset: - 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 drehe_robot(self, grad=90): - if self.typ == "backstein": - radius=9.5 - stift_versatz=2.2 - if self.typ == "brickies": - radius=17.4 - stift_versatz=0.3 - self.fahre_gerade(-radius - stift_versatz) - self.drehe(grad) - self.fahre_gerade(radius - stift_versatz) - - def fahre_gerade(self, cm, zeichne=False): - if zeichne: - self.bewege_stift(1) # Stift runter - self.motionSensor.reset_yaw_angle() - if self.typ == "brickies": - cm = -cm - self.movementMotors.move(cm) - if zeichne: - self.bewege_stift(-1) # Stift hoch - versatz = self.motionSensor.get_yaw_angle() - self.drehe(grad=-versatz) - - def buchstabe_zu_segmenten(self, buchstabe): - # Segmente um Buchstaben zu schreiben - # 4_ - # 5 |__|3 - # 0 |6_|2 - # 1 - # - buchstabe_zu_segmenten = {"L": [1,1,0,0,0,1,0], "E": [1,1,0,0,1,1,1], "G": [1,1,1,0,1,1,0], "O": [1,1,1,1,1,1,0]} - return buchstabe_zu_segmenten[buchstabe] - - - def bewege_stift(self, richtung): - if self.typ == "backstein": - self.frontMotorRight.run_for_rotations(richtung*0.4) - if self.typ == "brickies": - #print("bewege stift brickies") - self.bothFrontMotors.move(-richtung*0.2, unit='rotations', speed=5) - - def schreibe_buchstabe(self, buchstabe): - print("Schreibe " + buchstabe) - segmente = self.buchstabe_zu_segmenten(buchstabe) - grad_drehung=-90 - self.fahre_gerade(2) - 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.fahre_gerade(5, zeichne=True) - else: - self.fahre_gerade(5) - if segment_nummer != 2 and segment_nummer != 6: - self.drehe_robot(grad_drehung) # drehe links - - - def schreibeL(self, schreibe=True, zurueck=False): - if zurueck: - step = 5 - faktor = -1 - else: - step = 1 - faktor = 1 - print("Schreibe L") - #self.frontMotorRight.run_for_rotations(-0.4) - 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 schreibeLego(self): - #self.schreibeL() - #self.schreibeL(schreibe=False, zurueck=True) - self.movementMotors.set_default_speed(10) - self.bewege_stift(-1) - self.fahre_gerade(4, zeichne=True) - self.drehe_robot() - self.fahre_gerade(4, zeichne=True) - #self.schreibe_buchstabe("L") - #self.schreibe_buchstabe("E") - #self.schreibe_buchstabe("G") - #self.schreibe_buchstabe("O") - -print("successfully loaded the IQ Lego teams code :)") - - diff --git a/main.py b/main.py index 296fcf8..c6ba5c2 100644 --- a/main.py +++ b/main.py @@ -70,16 +70,22 @@ 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' + # Initialisieren des Hubs, der Aktoren und Sensoren hub = PrimeHub() # Initialisiere Robot Klasse mit unseren Funktionen -iqRobot: iq.IQRobot = iq.IQRobot(hub, typ=iq.BRICKIES_BOT) +iqRobot: iq.IQRobot = iq.IQRobot(hub, LEFT_MOTOR_PORT, RIGHT_MOTOR_PORT, COLOR_SENSOR_PORT) # Führe Funktionen aus unser Robot Klasse aus: iqRobot.show('HAPPY') -iqRobot.fahre_mit_drehung(strecke1=10, grad=90, strecke2=10) -iqRobot.fahre_mit_drehung(strecke1=0, grad=-90, strecke2=10) +iqRobot.driveForward_for_sec(2.0) +colorIntensity = iqRobot.getColorIntensity() +print("Farbintensität: " + str(colorIntensity))