prep to switch to actual pid

ROAR/agent_module/line_following_agent_2.py

@@ -30,14 +30,14 @@ def __init__(self, vehicle: Vehicle, agent_settings: AgentConfig, **kwargs):
 
     def run_step(self, sensors_data: SensorsData, vehicle: Vehicle) -> VehicleControl:
         super().run_step(sensors_data=sensors_data, vehicle=vehicle)
-        if self.front_depth_camera.data is not None:
-            depth_data = self.front_depth_camera.data
-            roi = depth_data[3*depth_data.shape[0]//4:, :]
-            cv2.imshow("roi", roi)
-            cv2.imshow("depth", depth_data)
-            dist = np.average(roi)
-            if dist < 0.25:
-                return VehicleControl(throttle=0, steering=0)
+        # if self.front_depth_camera.data is not None:
+        #     depth_data = self.front_depth_camera.data
+        #     roi = depth_data[3*depth_data.shape[0]//4:, :]
+        #     # cv2.imshow("roi", roi)
+        #     # cv2.imshow("depth", depth_data)
+        #     dist = np.average(roi)
+        #     if dist < 0.25:
+        #         return VehicleControl(throttle=0, steering=0)
         if self.front_rgb_camera.data is not None:
             # make rgb and depth into the same shape
             data: np.ndarray = cv2.resize(self.front_rgb_camera.data.copy(),
@@ -110,9 +110,9 @@ def find_error_at(self, data, y_offset, error_scaling, lower_range, upper_range)
         mask_yellow = cv2.inRange(src=data, lowerb=(0, 140, 0), upperb=(250, 200, 80))
         mask = mask_red | mask_yellow
 
-        cv2.imshow("mask", mask)
-        cv2.imshow("mask_red", mask_red)
-        cv2.imshow("mask_yellow", mask_yellow)
+        # cv2.imshow("mask", mask)
+        # cv2.imshow("mask_red", mask_red)
+        # cv2.imshow("mask_yellow", mask_yellow)
         kernel = np.ones((5, 5), np.uint8)
         mask = cv2.erode(mask, kernel, iterations=1)
         mask = cv2.dilate(mask, kernel, iterations=1)

ROAR/control_module/simple_pid_controller.py

@@ -15,13 +15,18 @@ def __init__(self, agent, **kwargs):
         self.lat_error_queue = deque(maxlen=lat_error_deque_length)  # this is how much error you want to accumulate
         self.long_error_queue = deque(maxlen=long_error_deque_length)  # this is how much error you want to accumulate
 
-        self.target_speed = 5  # m / s
+        self.target_speed = 10  # m / s
         ios_config_file_path = Path("ROAR_iOS/configurations/ios_config.json")
         self.ios_config: iOSConfig = iOSConfig.parse_file(ios_config_file_path)
 
-        self.lat_kp = 0.006  # this is how much you want to steer
+        # self.lat_kp = 0.006  # this is how much you want to steer
+        # self.lat_kd = 0.075  # this is how much you want to resist change
+        # self.lat_ki = 0.00025  # this is the correction on past error
+
+
+        self.lat_kp = 0.005  # this is how much you want to steer
         self.lat_kd = 0.075  # this is how much you want to resist change
-        self.lat_ki = 0.00025  # this is the correction on past error
+        self.lat_ki = 0.00025  # this is
 
         self.uphill_long_pid = {
             "long_kp": 0.25,
@@ -54,6 +59,12 @@ def lateral_pid_control(self) -> float:
         e_d = self.lat_kd * error_dt
         e_i = self.lat_ki * error_it
         lat_control = np.clip((e_p + e_d + e_i), -1, 1)
+        print(f"speed = {self.agent.vehicle.get_speed(self.agent.vehicle)}"
+              f"e = {round((e_p + e_d + e_i),3)}, "
+              f"e_p={round(e_p,3)},"
+              f"e_d={round(e_d,3)},"
+              f"e_i={round(e_i,3)},"
+              f"long_control={lat_control}")
         return lat_control
 
     def long_pid_control(self) -> float:
@@ -76,10 +87,11 @@ def long_pid_control(self) -> float:
         e_incline = 0.015 * incline
         total_error = e_p+e_d+e_i+e_incline
         long_control = np.clip(total_error, 0, self.ios_config.max_throttle)
-        print(f"e = {round(total_error,3)}, "
-              f"e_p={round(e_p,3)},"
-              f"e_d={round(e_d,3)},"
-              f"e_i={round(e_i,3)},"
-              f"e_incline={round(e_incline, 3)}, "
-              f"long_control={long_control}")
+        # print(f"speed = {self.agent.vehicle.get_speed(self.agent.vehicle)} "
+        #       f"e = {round(total_error,3)}, "
+        #       f"e_p={round(e_p,3)},"
+        #       f"e_d={round(e_d,3)},"
+        #       f"e_i={round(e_i,3)},"
+        #       f"e_incline={round(e_incline, 3)}, "
+        #       f"long_control={long_control}")
         return long_control

runner_ios.py

@@ -6,7 +6,7 @@
 from ROAR_Unity.unity_runner import iOSUnityRunner
 # from ROAR.agent_module.ios_agent import iOSAgent
 # from ROAR.agent_module.free_space_auto_agent import FreeSpaceAutoAgent
-# from ROAR.agent_module.line_following_agent_2 import LineFollowingAgent
+from ROAR.agent_module.line_following_agent_2 import LineFollowingAgent
 from ROAR.agent_module.special_agents.recording_agent import RecordingAgent
 from ROAR.agent_module.traffic_light_detector_agent import TrafficLightDectectorAgent
 from ROAR.agent_module.aruco_following_agent import ArucoFollowingAgent
@@ -115,7 +115,7 @@ def is_glove_online(host, port):
                 json.dump(ios_config.dict(), ios_config_file_path.open('w'), indent=4)
                 time.sleep(2)
         if success or args.reconnect is False:
-            agent = UDPMultiCastAgent(vehicle=Vehicle(), agent_settings=agent_config, should_init_default_cam=True)
+            agent = LineFollowingAgent(vehicle=Vehicle(), agent_settings=agent_config, should_init_default_cam=True)
             if args.use_unity:
                 runner = iOSUnityRunner(agent=agent, ios_config=ios_config)
             else: