diff --git a/GEMstack/knowledge/vehicle/dynamics.py b/GEMstack/knowledge/vehicle/dynamics.py index 9bd33a92e..3e097f4fc 100644 --- a/GEMstack/knowledge/vehicle/dynamics.py +++ b/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') diff --git a/GEMstack/knowledge/vehicle/gem_e4_dynamics.yaml b/GEMstack/knowledge/vehicle/gem_e4_dynamics.yaml index b1ca3c8c4..23712b0a5 100644 --- a/GEMstack/knowledge/vehicle/gem_e4_dynamics.yaml +++ b/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