LegoLeague/iqrobot.py

# LEGO type:standard slot:6 autostart

import math

from spike import PrimeHub, Motor, MotorPair, ColorSensor, MotionSensor, DistanceSensor
from spike.control import wait_for_seconds

print("Lade IQ-Bibliothek")


'''
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):
        
        self.hub: PrimeHub = hub

        # 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.farbSensor: ColorSensor = ColorSensor("C")
        self.bothFrontMotors: MotorPair = MotorPair(FRONT_MOTOR_LEFT_PORT, FRONT_MOTOR_RIGHT_PORT)
        self.linker_motor_vorne: Motor = Motor(FRONT_MOTOR_LEFT_PORT)
        self.linker_motor_vorne.set_stall_detection(stop_when_stalled=True)
        self.rechter_motor_vorne: Motor = Motor(FRONT_MOTOR_RIGHT_PORT)
        self.rechter_motor_vorne.set_stall_detection(stop_when_stalled=True)

        self.linker_motor: Motor = Motor(LEFT_MOTOR_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            
        self.rad_umfang = 2 * math.pi * self.rad_radius
        self.antrieb.set_motor_rotation(self.rad_umfang)

        self.bewegungsSensor: MotionSensor = MotionSensor()

        try: 
            self.abstandsSensor: DistanceSensor = DistanceSensor("D")
        except:
            self.abstandsSensor: DistanceSensor = DistanceSensor("C")

    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 strecke_gefahren(self):
        """
        Gibt die gefahrene Strecke basierend auf den Radumdrehungen zurück 
        """
        return -self.linker_motor.get_degrees_counted()/360 * self.rad_umfang


    def drehe(self, grad=90, with_reset=True, speed=10):
        """
        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
        :param bool with_reset: Parameter, um den Gierwinkel zurückzusetzen, Standard: True
        """

        # ist überhaupt etwas zu tun für uns? d.h. grad ist enweder 0 oder 360
        if grad == 0 or grad == 360 :
            print("nichts zu tun")
            return

        # soll der Gierwinkel zurückgesetzt werden?
        #if with_reset:
        #    self.bewegungsSensor.reset_yaw_angle() # Gierwinkel zurücksetzen
        
        #steering = 100 if grad > 0 else -100
        toleranz = 0 # Toleranz soll null sein. Kann erhöht werden, falls der Roboter sich unendlich dreht.
        aktuell = self.bewegungsSensor.get_yaw_angle() # Aktuelle Position
        if(aktuell + grad > 180):
            x1=180-aktuell
            x2=grad-x1
            x3=-180+x2
            ziel=x3
        else:
            ziel=aktuell+grad
        #ziel = grad
        steering = 100 if ziel > aktuell else -100
        self.antrieb.start(steering=steering, speed=speed) # Mit bestimmer Geschwindigkeit starten
        differenz = ziel - aktuell
        print ("Start Ziel: {}, Aktuell: {}".format(ziel, aktuell))

        # wiederhole solange der Grad der Drehung noch nicht erreicht ist
        while abs(differenz) > toleranz :
            print(str(differenz))
            aktuell = self.bewegungsSensor.get_yaw_angle()
            differenz = ziel - aktuell
            pass

        # stoppe die Bewegung
        self.antrieb.stop()
        print ("Final Ziel: {}, Aktuell: {}".format(ziel, aktuell))

    def drehe_bis_winkel(self, grad=90, speed=10):
        """
        Funktion um den Roboter auf der Stelle zu drehen

        :param int grad: Grad die der Roboter nachher haben soll
        mittels Vorzeichen +/- kann links oder rechts herum gedreht werden
        :param bool with_reset: Parameter, um den Gierwinkel zurückzusetzen, Standard: True
        """

        # ist überhaupt etwas zu tun für uns? d.h. grad ist enweder 0 oder 360
        if grad == self.get_grad() :
            print("Nichts zu tun")

        # soll der Gierwinkel zurückgesetzt werden?
        #if with_reset:
        #    self.bewegungsSensor.reset_yaw_angle() # Gierwinkel zurücksetzen
        
        #steering = 100 if grad > 0 else -100
        toleranz = 1 # Toleranz soll null sein. Kann erhöht werden, falls der Roboter sich unendlich dreht.
        aktuell = self.bewegungsSensor.get_yaw_angle() # Aktuelle Position
        ziel = grad
        steering = 100 if ziel > aktuell else -100
        self.antrieb.start(steering=steering, speed=speed) # Mit bestimmer Geschwindigkeit starten
        differenz = ziel - aktuell
        print ("Start Ziel: {}, Aktuell: {}".format(ziel, aktuell))

        # wiederhole solange der Grad der Drehung noch nicht erreicht ist
        while abs(differenz) > toleranz :
            print(str(differenz))
            aktuell = self.bewegungsSensor.get_yaw_angle()
            differenz = ziel - aktuell
            pass

        # stoppe die Bewegung
        self.antrieb.stop()
        print ("Final Ziel: {}, Aktuell: {}".format(ziel, aktuell))

    def fahre_gerade_aus(self, cm, speed=20, intensity_stop=None, abstand_stop=None, abstand_greater=True):
        """
        Funktion um den Roboter geradeaus fahren zu lassen

        :param int cm: Strecke in cm, die der Roboter geradeaus fahren soll
        :param int speed: Geschwindigkeit zum Fahren der Strecke, Standard: 20
        """

        # ist überhaupt etwas zu tun für uns? d.h. cm = 0
        if cm == 0 :
            print("nichts zu tun")
            return
        
        # wollen wir vorwärts oder rückwarts fahren?
        richtung = 1
        if cm < 0:
            richtung = -1
            speed = speed * richtung # Die Geschwindigkeit soll negativ sein, wenn wir rückwärts fahren

        # Alles zurücksetzen
        self.linker_motor.set_degrees_counted(0) 
        self.bewegungsSensor.reset_yaw_angle()

        # Mit irgendeiner Geschwindigkeit g>0 starten. Wert ist irrelevant
        self.antrieb.start_tank(richtung*10, richtung*10)
        self.antrieb.set_default_speed(richtung*10)

        linker_speed=speed      # Geschwindigkeit linker Motor
        rechter_speed=speed     # Geschwindigkeit rechter Motor
        #kp = 1.5                # Verstärkungsfaktor zur Regelung
        kp = 1.5               # 17.01.2024 ~Vincenz: Runtergesetzt
        ki = 1.0                # Integralfaktor zur Regelung
        sum_cm = 0              # bereits gefahrene Strecke
        versatz = 0             # aktueller Versatz
        sum_versatz = 0         # Summe des Versatzes über Zeit

        # wiederhole solange die gefahrene Strecke noch nicht erreicht ist
        while sum_cm < cm * richtung:            
            if intensity_stop:
                red, green, blue, intensity = self.farbSensor.get_rgb_intensity()
                print("Farbintensität: " + str(intensity))
                if intensity == intensity_stop:
                    break
            if abstand_stop:
                abstand = self.abstandsSensor.get_distance_cm()
                print("Abstand: " + str(abstand))
                if abstand_greater:
                    if abstand >= abstand_stop:
                        break
                else:
                    if abstand <= abstand_stop:
                        break
            wait_for_seconds(0.05) # Sonst wird das zu oft ausgeführt
            sum_cm = self.strecke_gefahren() * richtung # Gefahrene Strecke, ggf. eben negativ machen
            versatz = self.bewegungsSensor.get_yaw_angle() # Um wie viel sind wir falsch?
            sum_versatz = sum_versatz + versatz
            abweichung = (kp * versatz + ki * sum_versatz) / 100 # Abweichung berechnen, um zu korrigieren
            linker_speed = speed * (1 - abweichung * richtung) 
            rechter_speed = speed * (1 + abweichung * richtung)
            self.antrieb.start_tank_at_power(int(linker_speed), int(rechter_speed)) # Mit neuer Geschwindigkeit starten
            #print("Versatz: " + str(versatz) + " , linker Speed: " + str(linker_speed) + ", rechter Speed: " + str(rechter_speed) + ", strecke: " + str(sum_cm))

        self.antrieb.stop() # Stoppen
        self.drehe(-versatz) # Da Versatz immer != 0, korrigieren

    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)


    #deprecated
    def fahre_gerade_aus_alt(self, cm: float, speed: int):
        """
        Funktion zum gerade aus fahren mit Korrektur am Ende
        Wird nicht mehr aktiv genutzt, da wir jetzt fahre_gerade_aus haben, 
        welche geregelt ist, und der Roboter daher nicht schief wird.

        :param int cm: Zentimeter die gerade aus gefahren werden soll
        :param speed: Geschwindigkeit mit der gefahren wird
        """
    
        self.bewegungsSensor.reset_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.antrieb.move(amount=richtung * 0.1, steering=100)
            drehung = self.bewegungsSensor.get_yaw_angle()
            print(drehung)

    def heber(self, cm,speed):
        """
        Lässt den Heber fahren

        :param speed: Geschwindigkeit, mit der der Heber bewegt wird
        :param cm: Um wie viel soll der Heber bewegt werden?
        """
        self.bothFrontMotors.move_tank(cm*3.3,"cm", speed, speed) # Heber bewegen

    def schaufel(self,prozent, speed=100):
        """
        Lässt die Schaufel fahren

        :param prozent: Auf wie viel Prozent soll die Schaufel bewegt werden?
        """
        volle_umdrehung=0.29
        rotations=volle_umdrehung*prozent/100
        #self.bothFrontMotors.move(rotations, unit='rotations',speed=20)
        self.bothFrontMotors.move_tank(rotations, 'rotations', speed, -speed)

    # TODO: Geregeltes Fahren ist noch nicht eingebaut
    def fahre_bis_abstand(self, abstand: int, speed=30, geregelt=True):
        """
        Nutzt den Abstandssensor, um zu fahren, bis ein Abstand erreicht ist

        :param abstand: Abstand zum Objekt
        :param speed: Geschwindigkeit, mit der gefahren wird
        :param geregelt: Soll mit Regler gefahren werden?
        2"""
        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()
        
    def clear_light_matrix(self):
        """
        """
        for a in range(5):
            for b in range(5):
                self.hub.light_matrix.set_pixel(a, b, 0)

    
    def wait_for_any_press(self):
        self.clear_light_matrix()
        self.hub.light_matrix.set_pixel(4, 4, 100)
        self.hub.right_button.wait_until_pressed()
        self.clear_light_matrix()

    def write_light_matrix(self, str):
        self.hub.light_matrix.write(str)

    def get_grad(self):
        return self.hub.motion_sensor.get_yaw_angle()
        
print("Fertig geladen.")