Ackermann gefixt

ExoMy_Software-master/src/rover.py

@@ -18,6 +18,7 @@ class Rover():
 
     def __init__(self):
         self.locomotion_mode = LocomotionMode.FAKE_ACKERMANN
+        self.ackermann_straight_tolerance_deg = 5
 
         # x = Achsabstand, y = Spurbreite
         self.wheel_rx = 14.0
@@ -34,6 +35,9 @@ class Rover():
             self.wheel_fx) / math.tan(max_steering_angle * math.pi / 180.0) + (self.wheel_fy / 2)
         self.ackermann_r_min = max(self.ackermann_fr_min, self.ackermann_rr_min)
 
+    def is_ackermann_straight(self, steering_command):
+        return abs(abs(steering_command) - 90) <= self.ackermann_straight_tolerance_deg
+
     def setLocomotionMode(self, locomotion_mode_command):
         '''
         Sets the locomotion mode
@@ -121,7 +125,7 @@ class Rover():
             if(driving_command == 0):
                 return steering_angles
 
-            if math.cos(math.radians(steering_command)) == 0:
+            if self.is_ackermann_straight(steering_command):
                 return steering_angles
 
             radius = self.ackermann_r_max - \
@@ -231,6 +235,9 @@ class Rover():
             if (v == 0):
                 return motor_speeds
 
+            if self.is_ackermann_straight(steering_command):
+                return [v] * 6
+
             if (radius == self.ackermann_r_max):
                 return [v] * 6
             else: