Test backward control

Can run backward in simulation
Cannot combine forward and backward because it uses fixed_route and desired_speed is fixed. Positive desired_speed means forward and negative means backward.
Cannot change the negative velocity (fixed to -0.25m/s).

Author: udymd

GEMstack/knowledge/routes/backward_15m.csv

@@ -0,0 +1,11 @@
+0.0,0,0
+-1.0,0,0
+-2.0,0.1,0
+-3.0,0.2,0
+-4,0.4,0
+-5,0.8,0
+-6,1,0
+-7,1,0
+-8,1,0
+-9,1,0
+-10,1,0

GEMstack/knowledge/vehicle/dynamics.py

@@ -116,7 +116,16 @@ def apply_function(value, function='linear'):
         if abs(acceleration) < acceleration_deadband:
             #deadband?
             return (0,0,gear)
-        if velocity * acceleration < 0:
+        
+
+
+        # Revised by Summoning
+        # if velocity * acceleration < 0:
+        if velocity * acceleration < 0 and gear == 1:
+
+
+
+
             accel_pos = 0
             brake_pos = -acceleration / brake_max
             if brake_pos > 1.0:

GEMstack/knowledge/vehicle/gem_e4_dynamics.yaml

@@ -12,7 +12,7 @@ max_accelerator_power:  #Watts.  Power at max accelerator pedal, by gear (update
   - 5000.0
 max_accelerator_power_reverse: 5000.0 #Watts.  Power (backwards) in reverse gear
 
-acceleration_model : hang_v1
+acceleration_model : kris_v1
 accelerator_active_range : [0.2, 1.0]   #range of accelerator pedal where output acceleration is not flat
 brake_active_range : [0.5,1]             #range of brake pedal where output deceleration is not flat
 

GEMstack/onboard/interface/gem_simulator.py

@@ -174,6 +174,16 @@ def simulate(self, T : float, command : Optional[GEMVehicleCommand]):
         brake_pedal_position = np.clip(self.last_command.brake_pedal_position,0.0,1.0)
         acceleration = pedal_positions_to_acceleration(accelerator_pedal_position,brake_pedal_position,v,0,1)
         acceleration = np.clip(acceleration,*self.dubins.accelRange)
+
+
+
+        # Revised by Summoning
+        if self.last_command.gear < 0:
+            acceleration = -acceleration
+
+
+
+
         phides = steer2front(self.last_command.steering_wheel_angle)
         phides = np.clip(phides,*self.dubins.wheelAngleRange)
         h = 0.01  #just for finite differencing
@@ -207,10 +217,21 @@ def simulate(self, T : float, command : Optional[GEMVehicleCommand]):
             reading.brake_pedal_position = brake_pedal_position
             reading.accelerator_pedal_position = 0
         reading.speed = v
+
+
+
+
         if v >= 0:
             reading.gear = 1
         else:
             reading.gear = -1
+        # Revised by Summoning
+        # reading.gear = self.last_command.gear
+
+
+
+
+
         #copy signals
         reading.left_turn_signal = self.last_command.left_turn_signal
         reading.right_turn_signal = self.last_command.right_turn_signal

launch/fixed_route.yaml

@@ -16,13 +16,14 @@ drive:
     planning:
         route_planning:
             type: StaticRoutePlanner
-            args: [!relative_path '../GEMstack/knowledge/routes/xyhead_highbay_backlot_p.csv']
+            # args: [!relative_path '../GEMstack/knowledge/routes/xyhead_highbay_backlot_p.csv']
+            args: [!relative_path '../GEMstack/knowledge/routes/backward_15m.csv','start']
         motion_planning:
             type: RouteToTrajectoryPlanner
             args: [null]  #desired speed in m/s.  If null, this will keep the route untimed for the trajectory tracker
         trajectory_tracking:
             type: pure_pursuit.PurePursuitTrajectoryTracker
-            args: {desired_speed: 2.5}  #approximately 5mph
+            args: {desired_speed: -2.5}  #approximately 5mph
             print: False
 log:
     # Specify the top-level folder to save the log files.  Default is 'logs'
@@ -56,7 +57,7 @@ replay:  # Add items here to set certain topics / inputs to be replayed from log
 computation_graph: !include "../GEMstack/knowledge/defaults/computation_graph.yaml"
 
 after:
-    show_log_folder: True  #set to false to avoid showing the log folder
+    show_log_folder: False  #set to false to avoid showing the log folder
 
 #on load, variants will overload the settings structure
 variants: