Regelung der Geschwindigkeit besser

ExoMy_Software-master/src/joystick_parser_node.py

@@ -12,6 +12,17 @@ global motors_enabled
 locomotion_mode = LocomotionMode.ACKERMANN.value
 motors_enabled = True
 
+AXIS_DEADZONE = 0.1
+VELOCITY_EXPO = 2.0
+
+
+def apply_deadzone(value, deadzone):
+    if abs(value) <= deadzone:
+        return 0.0
+
+    scaled = (abs(value) - deadzone) / (1.0 - deadzone)
+    return math.copysign(scaled, value)
+
 
 def callback(data):
 
@@ -30,8 +41,8 @@ def callback(data):
     # START Button  | Enable and disable motors
 
     # Reading out joystick data
-    y = data.axes[1]
+    y = apply_deadzone(data.axes[1], AXIS_DEADZONE)
-    x = data.axes[0]
+    x = apply_deadzone(data.axes[0], AXIS_DEADZONE)
 
     # Reading out button data to set locomotion mode
     # X Button
@@ -66,7 +77,8 @@ def callback(data):
     rover_cmd.motors_enabled = motors_enabled
 
     # The velocity is decoded as value between 0...100
-    rover_cmd.vel = 100 * min(math.sqrt(x*x + y*y), 1.0)
+    stick_length = min(math.sqrt(x*x + y*y), 1.0)
+    rover_cmd.vel = 100 * math.pow(stick_length, VELOCITY_EXPO)
 
     # The steering is described as an angle between -180...180
     # Which describe the joystick position as follows: