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

Anpassung fahre_gerade_aus für negative Strecken

Browse source 
Signed-off-by: Harry Kimpel <harrykimpel@hotmail.com>
This commit is contained in:
Harry Kimpel 2023-11-29 21:14:04 +01:00
parent 2fe0c0a556
commit 147fb0cc8a
1 changed files with 11 additions and 10 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

21
iqrobot.py
View file

@ -106,6 +106,7 @@ class IQRobot:
richtung = 1 richtung = 1
if cm < 0: if cm < 0:
richtung = -1 richtung = -1
speed = speed * richtung
self.linker_motor.set_degrees_counted(0) self.linker_motor.set_degrees_counted(0)
self.bewegungsSensor.reset_yaw_angle() self.bewegungsSensor.reset_yaw_angle()
@ -113,23 +114,23 @@ class IQRobot:
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 * richtung # Geschwindigkeit linker Motor linker_speed=speed # Geschwindigkeit linker Motor
rechter_speed=speed * richtung # Geschwindigkeit rechter Motor rechter_speed=speed # Geschwindigkeit rechter Motor
kp = 1.5 # Verstärkungsfaktor zur Regelung kp = 1.5 # Verstärkungsfaktor zur Regelung
ki = 1.0 # Integralfaktor zur Regelung ki = 1.0 # 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
# 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) wait_for_seconds(0.05)
sum_cm = self.strecke_gefahren() sum_cm = self.strecke_gefahren() * richtung
versatz = self.bewegungsSensor.get_yaw_angle() versatz = self.bewegungsSensor.get_yaw_angle()
sum_versatz = sum_versatz + versatz sum_versatz = sum_versatz + versatz
abweichung = (kp * versatz + ki * sum_versatz) / 100 abweichung = (kp * versatz + ki * sum_versatz) / 100
linker_speed = speed * richtung * (1 - abweichung) linker_speed = speed * (1 - abweichung * richtung)
rechter_speed = speed * richtung * (1 + abweichung) rechter_speed = speed * (1 + abweichung * richtung)
self.antrieb.start_tank_at_power(int(linker_speed), int(rechter_speed)) 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)) #print("Versatz: " + str(versatz) + " , linker Speed: " + str(linker_speed) + ", rechter Speed: " + str(rechter_speed) + ", strecke: " + str(sum_cm))