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

Rückwärtsfahren mit Regler #16

Merged
viernst merged 1 commit from fixes into main 2023-11-29 17:59:22 +00:00
Conversation 0 Commits 1 Files changed 1 +6 -5
1 changed files with 6 additions and 5 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

11
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
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,8 +114,8 @@ 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
@ -124,12 +125,12 @@ class IQRobot:
# 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))