diff --git a/ExoMy_Software-master/src/joystick_parser_node.py b/ExoMy_Software-master/src/joystick_parser_node.py index 971b616..6ad141c 100644 --- a/ExoMy_Software-master/src/joystick_parser_node.py +++ b/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] - x = data.axes[0] + y = apply_deadzone(data.axes[1], AXIS_DEADZONE) + 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: