diff --git a/iqrobot.py b/iqrobot.py index d2750f3..3aa2ed9 100644 --- a/iqrobot.py +++ b/iqrobot.py @@ -2,10 +2,13 @@ import math -from spike import PrimeHub, Motor, MotorPair, ColorSensor, MotionSensor, DistanceSensor +from spike import PrimeHub, Motor, MotorPair, ColorSensor, MotionSensor 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` @@ -13,34 +16,53 @@ damit man dann später auch wieder Code Completion hat bei Nutzung der Variablen ''' class IQRobot: - def __init__(self, hub: PrimeHub): + def __init__(self, hub: PrimeHub, typ: str): self.hub: PrimeHub = hub + self.typ=typ - # Radantrieb - LEFT_MOTOR_PORT = 'E' - RIGHT_MOTOR_PORT = 'F' + 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) - # 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) + 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 - # 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") + 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() def show(self, image: str): @@ -51,114 +73,89 @@ class IQRobot: self.hub.light_matrix.show_image(image) - def strecke_gefahren(self): - return -self.linker_motor.get_degrees_counted()/360 * self.rad_umfang + 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): - """ - 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.bewegungsSensor.reset_yaw_angle() + self.motionSensor.reset_yaw_angle() #steering = 100 if grad > 0 else -100 toleranz = 0 - aktuell = self.bewegungsSensor.get_yaw_angle() + aktuell = self.motionSensor.get_yaw_angle() ziel = grad steering = 100 if ziel > aktuell else -100 - self.antrieb.start(steering=steering, speed=10) + self.movementMotors.start(steering=steering, speed=10) differenz = ziel - aktuell print ("Start Ziel: {}, Aktuell: {}".format(ziel, aktuell)) while abs(differenz) > toleranz : - aktuell = self.bewegungsSensor.get_yaw_angle() + aktuell = self.motionSensor.get_yaw_angle() differenz = ziel - aktuell pass - self.antrieb.stop() + self.movementMotors.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() - self.antrieb.start_tank(10, 10) - self.antrieb.set_default_speed(10) + 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) - linker_speed=speed - rechter_speed=speed - kp = 1.5 - ki = 1.0 sum_cm = 0 - sum_versatz = 0 while sum_cm < cm: - 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(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): - """ - 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_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 - """ + def fahre_gerade_aus(self, cm,speed): - self.bewegungsSensor.reset_yaw_angle() + self.motionSensor.reset_yaw_angle() - self.antrieb.move_tank(amount=cm,left_speed=speed, right_speed=speed) - drehung = self.bewegungsSensor.get_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.antrieb.move(amount=richtung * 0.1, steering=100) - drehung = self.bewegungsSensor.get_yaw_angle() + self.movementMotors.move(amount=richtung * 0.1, steering=100) + drehung = self.motionSensor.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 :)") @@ -170,4 +167,3 @@ print("successfully loaded the IQ Lego teams code :)") - diff --git a/main.py b/main.py index d5ac33c..3908dbc 100644 --- a/main.py +++ b/main.py @@ -74,10 +74,12 @@ dh auch an die Funktionen im importierten Code übergeben werde hub = PrimeHub() # Initialisiere Robot Klasse mit unseren Funktionen -iqRobot: iq.IQRobot = iq.IQRobot(hub) +iqRobot: iq.IQRobot = iq.IQRobot(hub, typ=iq.BRICKIES_BOT) # 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) @@ -87,11 +89,7 @@ def huenchenaufgabe(self): iqRobot.fahre_gerade_aus(55,60) iqRobot.heber(10,30) -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) +huenchenaufgabe() + +