diff --git a/iqrobot.py b/iqrobot.py index 3aa2ed9..d2750f3 100644 --- a/iqrobot.py +++ b/iqrobot.py @@ -2,13 +2,10 @@ import math -from spike import PrimeHub, Motor, MotorPair, ColorSensor, MotionSensor +from spike import PrimeHub, Motor, MotorPair, ColorSensor, MotionSensor, DistanceSensor 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 +13,34 @@ 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): 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) + # Radantrieb + LEFT_MOTOR_PORT = 'E' + RIGHT_MOTOR_PORT = 'F' - 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) + # Motoren für Aufsätze + FRONT_MOTOR_RIGHT_PORT = "B" + FRONT_MOTOR_LEFT_PORT = "A" + + self.bothFrontMotors: MotorPair = MotorPair(FRONT_MOTOR_LEFT_PORT, FRONT_MOTOR_RIGHT_PORT) + + self.linker_motor: Motor = Motor(LEFT_MOTOR_PORT) + + self.antrieb: 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() + # Radius der Antriebsräder + self.rad_radius = 2.1 + # Abstand zwischen Rädern (Mitte) und Vorderseite des Roboters + self.abstand_rad_front = 5.55 + self.rad_umfang = 2 * math.pi * self.rad_radius + self.antrieb.set_motor_rotation(self.rad_umfang) + + self.bewegungsSensor: MotionSensor = MotionSensor() + + self.abstandsSensor: DistanceSensor = DistanceSensor("D") def show(self, image: str): @@ -73,89 +51,114 @@ class IQRobot: 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 strecke_gefahren(self): + return -self.linker_motor.get_degrees_counted()/360 * self.rad_umfang def drehe(self, grad=90, with_reset=True): + """ + Funktion um den Roboter auf der Stelle zu drehen + + :param int grad: Grad um die der Roboter gedreht werden soll + mittels Vorzeichen +/- kann links oder rechts herum gedreht werden + """ if grad == 0 or grad == 360 : print("nichts zu tun") return if with_reset: - self.motionSensor.reset_yaw_angle() + self.bewegungsSensor.reset_yaw_angle() #steering = 100 if grad > 0 else -100 toleranz = 0 - aktuell = self.motionSensor.get_yaw_angle() + aktuell = self.bewegungsSensor.get_yaw_angle() ziel = grad steering = 100 if ziel > aktuell else -100 - self.movementMotors.start(steering=steering, speed=10) + self.antrieb.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() + aktuell = self.bewegungsSensor.get_yaw_angle() differenz = ziel - aktuell pass - self.movementMotors.stop() + self.antrieb.stop() print ("Final Ziel: {}, Aktuell: {}".format(ziel, aktuell)) + + def fahre_gerade_aus(self, cm, speed=20): + self.linker_motor.set_degrees_counted(0) + self.bewegungsSensor.reset_yaw_angle() - 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) + self.antrieb.start_tank(10, 10) + self.antrieb.set_default_speed(10) + linker_speed=speed + rechter_speed=speed + kp = 1.5 + ki = 1.0 sum_cm = 0 + sum_versatz = 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 + wait_for_seconds(0.05) + sum_cm = self.strecke_gefahren() + versatz = self.bewegungsSensor.get_yaw_angle() + sum_versatz = sum_versatz + versatz + abweichung = (kp * versatz + ki * sum_versatz) / 100 + linker_speed = speed * (1 - abweichung) + rechter_speed = speed * (1 + abweichung) + self.antrieb.start_tank_at_power(int(linker_speed), int(rechter_speed)) + #print("Versatz: " + str(versatz) + " , linker Speed: " + str(linker_speed) + ", rechter Speed: " + str(rechter_speed) + ", strecke: " + str(sum_cm)) + + self.antrieb.stop() + self.drehe(-versatz) - self.movementMotors.move(cm - sum_cm) def fahre_mit_drehung(self, strecke1, grad, strecke2): + """ + Funktion für eine Fahrt mit 1. Strecke, dann Drehung in der Mitte, dann 2. Strecke + Vereinfacht die Logik, da der Roboter durch die Drehung einen Versatz hat gegenüber einer + Strecke die mit dem Lineal ausgemessen wurde + """ 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): + + def fahre_gerade_aus_alt(self, cm: float, speed: int): + """ + Funktion zum gerade aus fahren mit Korrektur am Ende + + :param int cm: Zentimeter die gerade aus gefahren werden soll + :param speed: Geschwindigkeit mit der gefahren wird + """ - self.motionSensor.reset_yaw_angle() + self.bewegungsSensor.reset_yaw_angle() - self.movementMotors.move_tank(amount=cm,left_speed=speed, right_speed=speed) - drehung = self.motionSensor.get_yaw_angle() + self.antrieb.move_tank(amount=cm,left_speed=speed, right_speed=speed) + drehung = self.bewegungsSensor.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() + self.antrieb.move(amount=richtung * 0.1, steering=100) + drehung = self.bewegungsSensor.get_yaw_angle() print(drehung) + + def heber(self, cm,speed): + self.bothFrontMotors.move_tank(-cm*3.3,"cm", -speed, speed) + + def schaufel(self,prozent): + volle_umdrehung=0.29 + rotations=volle_umdrehung*prozent/100 + self.bothFrontMotors.move(rotations, unit='rotations',speed=20) + + + def fahre_bis_abstand(self, abstand: int, speed=30, geregelt=True): + self.antrieb.start_at_power(speed) + abstand_gerade = self.abstandsSensor.get_distance_cm() + while abstand_gerade > abstand: + abstand_gerade = self.abstandsSensor.get_distance_cm() + print(str(abstand_gerade)) + self.antrieb.stop() print("successfully loaded the IQ Lego teams code :)") @@ -167,3 +170,4 @@ print("successfully loaded the IQ Lego teams code :)") + diff --git a/main.py b/main.py index 3908dbc..d5ac33c 100644 --- a/main.py +++ b/main.py @@ -74,12 +74,10 @@ dh auch an die Funktionen im importierten Code übergeben werde hub = PrimeHub() # Initialisiere Robot Klasse mit unseren Funktionen -iqRobot: iq.IQRobot = iq.IQRobot(hub, typ=iq.BRICKIES_BOT) +iqRobot: iq.IQRobot = iq.IQRobot(hub) # 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) def huenchenaufgabe(self): iqRobot.fahre_gerade_aus(40,60) @@ -89,7 +87,11 @@ def huenchenaufgabe(self): iqRobot.fahre_gerade_aus(55,60) iqRobot.heber(10,30) -huenchenaufgabe() - - +def hologram_aufgabe1(): + iqRobot.fahre_gerade_aus(cm=75,speed=80) + iqRobot.drehe(45, False) + iqRobot.fahre_gerade_aus(cm=14,speed=70) + iqRobot.fahre_gerade_aus(cm=-13,speed=50) + iqRobot.drehe(-45, False) + iqRobot.fahre_gerade_aus(cm=-75,speed=50)