Projekte/LegoLeague
3
0
Fork 0
Code Issues 2 Pull requests Projects Releases Packages Wiki Activity

Beschleunigung beim Fahren

Browse source
This commit is contained in:
(MakerLab) Laptop 2 2024-02-28 19:04:29 +01:00
parent 37c7c0aefd
commit 72da85c3e3
1 changed files with 14 additions and 6 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

20
brickiesbot.py
View file

@ -126,28 +126,36 @@ class IQRobot:
self.bewegungsSensor.reset_yaw_angle() self.bewegungsSensor.reset_yaw_angle()
# Mit irgendeiner Geschwindigkeit g>0 starten. Wert ist irrelevant # 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) self.antrieb.set_default_speed(10)
linker_speed=speed # Geschwindigkeit linker Motor ziel_speed=speed
rechter_speed=speed # Geschwindigkeit rechter Motor k_speed=0.04 # Beschleunigung in %
kp = 1.1 # Verstärkungsfaktor zur Regelung
ki = 1.0 # Integralfaktor zur Regelung # Regler Parameter: Nicht ändern!
kp = 5.0 # Verstärkungsfaktor zur Regelung
ki = 0.5 # Integralfaktor zur Regelung
sum_cm = 0 # bereits gefahrene Strecke sum_cm = 0 # bereits gefahrene Strecke
versatz = 0 # aktueller Versatz versatz = 0 # aktueller Versatz
sum_versatz = 0 # Summe des Versatzes über Zeit 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 # wiederhole solange die gefahrene Strecke noch nicht erreicht ist
while sum_cm < cm * richtung: while sum_cm < cm * richtung:
wait_for_seconds(0.05) # Sonst wird das zu oft ausgeführt 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 sum_cm = self.strecke_gefahren() * richtung # Gefahrene Strecke, ggf. eben negativ machen
versatz = self.bewegungsSensor.get_yaw_angle() # Um wie viel sind wir falsch? versatz = self.bewegungsSensor.get_yaw_angle() # Um wie viel sind wir falsch?
sum_versatz = sum_versatz + versatz sum_versatz = sum_versatz + versatz
abweichung = (kp * versatz + ki * sum_versatz) / 100 # Abweichung berechnen, um zu korrigieren abweichung = (kp * versatz + ki * sum_versatz) / 100 # Abweichung berechnen, um zu korrigieren
linker_speed = speed * (1 - abweichung * richtung) linker_speed = speed * (1 - abweichung * richtung)
rechter_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 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.antrieb.stop() # Stoppen
self.drehe(-versatz) # Da Versatz immer != 0, korrigieren self.drehe(-versatz) # Da Versatz immer != 0, korrigieren