diff --git a/brickiesbot.py b/brickiesbot.py index 5d27a4e..b329db2 100644 --- a/brickiesbot.py +++ b/brickiesbot.py @@ -126,28 +126,36 @@ class IQRobot: self.bewegungsSensor.reset_yaw_angle() # Mit irgendeiner Geschwindigkeit g>0 starten. Wert ist irrelevant - self.antrieb.start_tank(10, 10) + #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.1 # Verstärkungsfaktor zur Regelung - ki = 1.0 # Integralfaktor zur Regelung + ziel_speed=speed + k_speed=0.04 # Beschleunigung in % + + # Regler Parameter: Nicht ändern! + kp = 5.0 # Verstärkungsfaktor zur Regelung + ki = 0.5 # Integralfaktor zur Regelung sum_cm = 0 # bereits gefahrene Strecke versatz = 0 # aktueller Versatz sum_versatz = 0 # Summe des Versatzes über Zeit + self.antrieb.start_tank_at_power(50, 50) + speed = min(20, speed) # wiederhole solange die gefahrene Strecke noch nicht erreicht ist while sum_cm < cm * richtung: wait_for_seconds(0.05) # Sonst wird das zu oft ausgeführt + + speed = ziel_speed * k_speed + speed * (1 - k_speed) + 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)) + print("Versatz: " + str(versatz) + " , linker Speed: " + str(linker_speed) + ", rechter Speed: " + str(rechter_speed) + ", strecke: " + str(sum_cm) + ", sum versatz: " + str(sum_versatz)) self.antrieb.stop() # Stoppen self.drehe(-versatz) # Da Versatz immer != 0, korrigieren