Rewrote qualifying test & interface with new braking interface in mind.

ros2/cmd/src/modules/State_Machine_Interface.py

@@ -122,30 +122,24 @@ def Lane_Change_Action(self, args=None):
     # Those calculations should be done before_hand, in the state machines.
     def Cstop_Action(self, args=None):
         # We need to parametrize this, slope in m/s^2
-        slope = 1.32
         # d = (vf^2-vi^2)/2a
         # (vf is zero)
         # vi^2 / d = 2a
+        # if args is not None:
+        #     dist = args[0]
+        #     attempted_slope = (current_speed**2) / (2 * dist)
+        #     # Guaranteed that slope is not greater than 1.32
+        #     slope = min(attempted_slope, slope)
         current_speed = self.lane_follow.odom_sub.vel
-        if args is not None:
-            dist = args[0]
-            attempted_slope = (current_speed**2) / (2 * dist)
-            # Guaranteed that slope is not greater than 1.32
-            slope = min(attempted_slope, slope)
-
         cmd = Float32MultiArray()
-        initial = datetime.now()
         # Catching any precision errors & I'm not entirely sure how odom velocity is calculated
         # In other words, lazy programming. TODO: Determine appropriate bounds:)
         while current_speed > 0.05:
             current_speed = self.lane_follow.odom_sub.vel
-            current_time = datetime.now()
-            difference = (initial - current_time).total_seconds()
             cmd.data = [
                 0.0,
-                max(current_speed - (slope * difference), 0),
+                0.0,
             ]  # Allows us to keep slope @ set time
-            initial = current_time
             self.cmd_publisher.publish(cmd)
 
     # Estop Action: When things break. args[0] is "soft estop", no args or args[0] false is a hard estop.

ros2/cmd/src/state_machines/qualifying_tests/qualifying_test_q3.py

@@ -15,6 +15,7 @@ def __init__(self, interface: Interface):
 
     # State transistion logic
     def function_test(self):
+        OFFSET = 1.65 # Meter offset from barrel before we stop.
         barrel_counter = 0
         distance = 0
         self.interface.car_sm.Resume()
@@ -25,10 +26,12 @@ def function_test(self):
             if obj_data[0]:
                 distance = obj_data[1]
                 barrel_counter += 1
-            self.interface.Run()
+                self.interface.car_sm.Obj_Avoidance()
+                args = [distance-OFFSET,obj_data[2],0]
+                self.interface.run(args)
         # 0.9144 = 3 feet to meters
         self.interface.car_sm.Stop_Trigger()
-        self.interface.Run(distance - 0.9144)
+        self.interface.Run(distance - OFFSET)
 
 
 if __name__ == "__main__":