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

Gerade aus Fahr Funktion mit PI Regler

Browse source
This commit is contained in:
Lars Haferkamp 2023-11-22 18:56:19 +01:00
parent c40f2a97ed
commit cd47da6f23
1 changed files with 26 additions and 12 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

38
iqrobot.py
View file

@ -27,14 +27,16 @@ class IQRobot:
self.bothFrontMotors: MotorPair = MotorPair(FRONT_MOTOR_LEFT_PORT, FRONT_MOTOR_RIGHT_PORT) 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) self.antrieb: MotorPair = MotorPair(LEFT_MOTOR_PORT, RIGHT_MOTOR_PORT)
# Radumfang neu berechnen und Motor konfigurieren # Radumfang neu berechnen und Motor konfigurieren
# Radius der Antriebsräder # Radius der Antriebsräder
self.rad_radius = 2.1 self.rad_radius = 2.1
# Abstand zwischen Rädern (Mitte) und Vorderseite des Roboters # Abstand zwischen Rädern (Mitte) und Vorderseite des Roboters
self.abstand_rad_front = 5.55 self.abstand_rad_front = 5.55
rad_umfang = 2 * math.pi * self.rad_radius self.rad_umfang = 2 * math.pi * self.rad_radius
self.antrieb.set_motor_rotation(rad_umfang) self.antrieb.set_motor_rotation(self.rad_umfang)
self.bewegungsSensor: MotionSensor = MotionSensor() self.bewegungsSensor: MotionSensor = MotionSensor()
@ -47,6 +49,9 @@ class IQRobot:
self.hub.light_matrix.show_image(image) 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): def drehe(self, grad=90, with_reset=True):
""" """
Funktion um den Roboter auf der Stelle zu drehen Funktion um den Roboter auf der Stelle zu drehen
@ -75,23 +80,32 @@ class IQRobot:
print ("Final Ziel: {}, Aktuell: {}".format(ziel, aktuell)) print ("Final Ziel: {}, Aktuell: {}".format(ziel, aktuell))
def fahre_gerade_geregelt(self, cm): def fahre_gerade_aus(self, cm, speed=20):
""" self.linker_motor.set_degrees_counted(0)
WIP by Lars & Klaus
"""
self.bewegungsSensor.reset_yaw_angle() self.bewegungsSensor.reset_yaw_angle()
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
rechter_speed=speed
kp = 1.5
ki = 1.0
sum_cm = 0 sum_cm = 0
sum_versatz = 0
while sum_cm < cm: while sum_cm < cm:
self.antrieb.move(1) wait_for_seconds(0.05)
sum_cm = self.strecke_gefahren()
versatz = self.bewegungsSensor.get_yaw_angle() versatz = self.bewegungsSensor.get_yaw_angle()
self.drehe(grad=-versatz) sum_versatz = sum_versatz + versatz
self.bewegungsSensor.reset_yaw_angle() abweichung = (kp * versatz + ki * sum_versatz) / 100
sum_cm = sum_cm + 1 linker_speed = speed * (1 - abweichung)
rechter_speed = speed * (1 + abweichung)
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.move(cm - sum_cm) self.antrieb.stop()
self.drehe(-versatz)
def fahre_mit_drehung(self, strecke1, grad, strecke2): def fahre_mit_drehung(self, strecke1, grad, strecke2):
@ -105,7 +119,7 @@ class IQRobot:
self.fahre_gerade_geregelt(strecke2 - self.abstand_rad_front) self.fahre_gerade_geregelt(strecke2 - self.abstand_rad_front)
def fahre_gerade_aus(self, cm: float, speed: int): def fahre_gerade_aus_alt(self, cm: float, speed: int):
""" """
Funktion zum gerade aus fahren mit Korrektur am Ende Funktion zum gerade aus fahren mit Korrektur am Ende