Add additional model to dynamics.py that allows for various function mappings from the desired acceleration to pedal position. Also allows for variation in response based on current velocity. Adds corresponding settings into gem_e4_dynamics.yaml

Author: alo-20

GEMstack/knowledge/vehicle/dynamics.py

@@ -31,13 +31,63 @@ def acceleration_to_pedal_positions(acceleration : float, velocity : float, pitc
             if acceleration < -dry_decel*0.5 or (acceleration <= 0 and velocity < 0.1):  # a little deadband to avoid oscillation
                 throttle_percent = 0.0  #drift to stop
             else:
-                throttle_percent = accel_active_range[0] + (acceleration+dry_decel)/max_accel * (accel_active_range[1]-accel_active_range[0])
+                throttle_percent = accel_active_range[0] + ((acceleration+dry_decel)/max_accel * (accel_active_range[1]-accel_active_range[0]))
             brake_percent = 0
         else:
-            brake_percent = brake_active_range[0] + -(acceleration+dry_decel)/max_brake * (brake_active_range[1]-brake_active_range[0])
+            brake_percent = brake_active_range[0] + (-(acceleration+dry_decel)/max_brake * (brake_active_range[1]-brake_active_range[0]))
             throttle_percent = 0
+        print(acceleration, (max(throttle_percent,0.0),max(brake_percent,0.0),1))
         return (max(throttle_percent,0.0),max(brake_percent,0.0),1)
 
+
+    # Model that's built on top of Hang's model.
+    # Designed to allow for various scaling based on current vehicle velocity, and use of functions other than linear
+    # for scaling. 
+    # DANGER: NEEDS TESTED
+    elif model == 'avery_v1':
+        model = settings.get('vehicle.dynamics.acceleration_model', 'avery_v1')
+        if gear != 1:
+            print("WARNING: Can't handle gears other than 1 yet")
+        
+        max_accel = settings.get('vehicle.dynamics.max_accelerator_acceleration')[1]  # m/s^2
+        max_brake = settings.get('vehicle.dynamics.max_brake_deceleration')  # m/s^2
+        dry_decel = settings.get('vehicle.dynamics.internal_dry_deceleration')  # m/s^2
+        accel_active_range = settings.get('vehicle.dynamics.accelerator_active_range')  # pedal position fraction
+        brake_active_range = settings.get('vehicle.dynamics.brake_active_range')  # pedal position fraction
+        mapping_function = settings.get('vehicle.dynamics.pedal_mapping_function', 'linear')
+        velocity_scalar = settings.get('vehicle.dynamics.velocity_scaling_factor', 0)  # Adjust sensitivity based on velocity
+        
+        
+        def apply_function(value, function='linear'):
+            if function == 'linear':
+                value = value # already linear mapping
+
+            # no guarantee that these functions will give good results until we get more test data about accel -> pedal mapping.
+            elif function == 'quadratic':
+                value = value ** 2 # value squared
+            elif function == 'exponential':
+                value = (math.exp(value) - 1) / 2
+            elif function == 'log':
+                value = math.sqrt(math.log(value + 1)) + 0.2
+
+            value += velocity * velocity_scalar
+            return value
+        
+        if acceleration > -dry_decel:
+            if acceleration < -dry_decel*0.5 or (acceleration <= 0 and velocity < 0.1):  # a little deadband to avoid oscillation
+                throttle_percent = 0.0  #drift to stop
+            else:
+                throttle_percent = accel_active_range[0] + ((acceleration+dry_decel)/max_accel * (accel_active_range[1]-accel_active_range[0]))
+                throttle_percent = apply_function(throttle_percent, mapping_function)
+            brake_percent = 0
+        else:
+            brake_percent = brake_active_range[0] + (-(acceleration+dry_decel)/max_brake * (brake_active_range[1]-brake_active_range[0]))
+            brake_percent = apply_function(brake_percent, mapping_function)
+            throttle_percent = 0
+        print(acceleration, (max(throttle_percent,0.0),max(brake_percent,0.0),1))
+        return (max(throttle_percent,0.0),max(brake_percent,0.0),1)
+
+
     elif model == 'kris_v1':
         brake_max = settings.get('vehicle.dynamics.max_brake_deceleration')
         reverse_accel_max = settings.get('vehicle.dynamics.max_accelerator_acceleration_reverse')

GEMstack/knowledge/vehicle/gem_e4_dynamics.yaml

@@ -20,3 +20,5 @@ internal_dry_deceleration: 0.2      #m/s^2: deceleration due to internal dry fri
 internal_viscous_deceleration: 0.05  #1/s: scales the current velocity to get deceleration due to internal viscous friction (speed dependent)
 aerodynamic_drag_coefficient: 0.01  #units in s, scaled by velocity^2 to get deceleration due to aerodynamic drag
 acceleration_deadband: 0.1          #m/s^2: minimum acceleration to be considered non-zero
+velocity_scaling_factor: 0.0           # Velocity * this scalar will be added to brake and throttle percent. 
+pedal_mapping_function: linear