diff --git a/iqrobot.py b/iqrobot.py index 3aa2ed9..6a4791c 100644 --- a/iqrobot.py +++ b/iqrobot.py @@ -6,9 +6,6 @@ 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` @@ -16,53 +13,30 @@ 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.antrieb: 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 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.antrieb.set_motor_rotation(rad_umfang) + + self.bewegungsSensor: MotionSensor = MotionSensor() def show(self, image: str): @@ -73,88 +47,84 @@ 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 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(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) + """ + WIP by Lars & Klaus + """ + self.bewegungsSensor.reset_yaw_angle() + self.antrieb.start_tank(10, 10) + self.antrieb.set_default_speed(10) sum_cm = 0 while sum_cm < cm: - self.movementMotors.move(1) - versatz = self.motionSensor.get_yaw_angle() + self.antrieb.move(1) + versatz = self.bewegungsSensor.get_yaw_angle() self.drehe(grad=-versatz) - self.motionSensor.reset_yaw_angle() + self.bewegungsSensor.reset_yaw_angle() sum_cm = sum_cm + 1 - self.movementMotors.move(cm - sum_cm) + self.antrieb.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(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) diff --git a/main.py b/main.py index 682b717..cffdfc7 100644 --- a/main.py +++ b/main.py @@ -74,7 +74,7 @@ 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_2) +iqRobot: iq.IQRobot = iq.IQRobot(hub) # Führe Funktionen aus unser Robot Klasse aus: iqRobot.show('HAPPY')