autopilot.py

@@ -326,7 +326,7 @@ class AutoPilot(Pilot):
         distance_to_target = math.sqrt(
             (self.points[self.target_idx][0] - self.pos.x) ** 2 +
             (self.points[self.target_idx][1] - self.pos.y) ** 2
-        )
+        ) * self.pixel_ratio
 
         if distance_to_target < 35:
             self.target_idx += 1
@@ -339,10 +339,32 @@ class AutoPilot(Pilot):
                 (self.points[self.target_idx][1] - self.pos.y) ** 2
             )
 
+        angle_trajectory = self.pos.yaw + math.pi / 2
+
+        # Проверка на слепую зону
+        R = 120
+        blind = np.array([
+            [
+                self.pos.x * self.pixel_ratio + R * np.cos(angle_trajectory - np.pi / 2),
+                self.pos.y * self.pixel_ratio + R * np.sin(angle_trajectory - np.pi / 2),
+            ],
+            [
+                self.pos.x * self.pixel_ratio + R * np.cos(angle_trajectory + np.pi / 2),
+                self.pos.y * self.pixel_ratio + R * np.sin(angle_trajectory + np.pi / 2),
+            ]
+        ])
+
+        blind -= self.points[self.target_idx] * self.pixel_ratio
+        blind = np.hypot(blind[:, 0], blind[:, 1])
+
+        print("R: ", blind)
+        if np.min(blind) < R:
+            return PilotCommand(0, 10, False, self.timer.get_elapsed())
+
+        
+
         # Вычисляем угол к цели
         target_angle = math.atan2(self.points[self.target_idx][1] - self.pos.y, self.points[self.target_idx][0] - self.pos.x)
-        
-        angle_trajectory = self.pos.yaw + math.pi / 2
 
         # Вычисляем разность углов (направление поворота)
         angle_diff = target_angle - angle_trajectory
@@ -352,7 +374,7 @@ class AutoPilot(Pilot):
         if angle_diff >= math.pi:
             angle_diff -= 2 * math.pi
 
-        d_r = max(5, min(10., distance_to_target / 2 * self.pixel_ratio))
+        d_r = max(5, min(10., distance_to_target / 2))
         d_a_limit = np.radians(5)
 
         command = PilotCommand(