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

HELLO = "HELLO IQ"

'''
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.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
        # 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):
        '''
        Zeige Bild auf LED Matrix des Spikes
        image: Bildname wie zB 'HAPPY'
        '''
        self.hub.light_matrix.show_image(image)

    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
        :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()
        
        #steering = 100 if grad > 0 else -100
        toleranz = 0
        aktuell = self.bewegungsSensor.get_yaw_angle()
        ziel = grad
        steering = 100 if ziel > aktuell else -100
        self.antrieb.start(steering=steering, speed=10)
        differenz = ziel - aktuell
        print ("Start Ziel: {}, Aktuell: {}".format(ziel, aktuell))

        # wiederhole solange der Grad der Drehung noch nicht erreicht ist
        while abs(differenz) > toleranz :
            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):
        """
        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

        self.linker_motor.set_degrees_counted(0)
        self.bewegungsSensor.reset_yaw_angle()

        self.antrieb.start_tank(10, 10)
        self.antrieb.set_default_speed(10)

        linker_speed=speed              # Geschwindigkeit linker Motor
        rechter_speed=speed             # Geschwindigkeit rechter Motor
        kp = 1.5                        # Verstärkungsfaktor zur Regelung
        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:
            wait_for_seconds(0.05)
            sum_cm = self.strecke_gefahren() * richtung
            versatz = self.bewegungsSensor.get_yaw_angle()
            sum_versatz = sum_versatz + versatz
            abweichung = (kp * versatz + ki * sum_versatz) / 100
            linker_speed = speed * (1 - abweichung * richtung) 
            rechter_speed = speed * (1 + abweichung * richtung)
            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)

    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
        """
    
        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):
        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 :)")