feat: build map and realize act method

autopilot.py

@@ -209,18 +209,18 @@ class AutoPilot(Pilot):
             transformation_info = self.estimate_transformation_matrix(src_pts, dst_pts)
             
             if transformation_info:
-                print(f"Frame {self.frame_count}:")
-                print(f"  Translation: {transformation_info['translation']}")
-                print(f"  Rotation: {transformation_info['rotation']:.4f} rad")
-                print(f"  Scale: {transformation_info['scale']:.4f}")
+                # print(f"Frame {self.frame_count}:")
+                # print(f"  Translation: {transformation_info['translation']}")
+                # print(f"  Rotation: {transformation_info['rotation']:.4f} rad")
+                # print(f"  Scale: {transformation_info['scale']:.4f}")
                 
                 # Обновляем позицию и угол БПЛА
                 self.update_drone_position(transformation_info)
                 
                 # Выводим текущее состояние БПЛА
                 drone_state = self.get_drone_state()
-                print(f"  Drone Position: ({drone_state['x']:.2f}, {drone_state['y']:.2f})")
-                print(f"  Drone Angle: {drone_state['angle_degrees']:.1f}°")
+                print(f"  [Pilot] Drone Position: ({drone_state['x']:.2f}, {drone_state['y']:.2f})")
+                print(f"  [Pilot] Angle: {drone_state['angle_degrees']:.1f}°")
                 
                 # Визуализация (опционально)
                 img_matches = self.visualize_matches(self.prev_image, current_image, 
@@ -231,9 +231,33 @@ class AutoPilot(Pilot):
         # Обновляем предыдущее изображение
         self.prev_image = current_image
 
-    def act(self) -> float:
-        """Возвращает угол поворота для управления дроном"""
-        return self.angle
+    def act(self) -> float | None:
+        """
+        Возвращает угол поворота для управления дроном, чтобы он стремился к точке (0, 0).
+        Если дрон находится рядом с началом координат (в радиусе 1 метра), возвращает None.
+        """
+        # Расстояние до цели (0, 0)
+        distance_to_target = math.sqrt(self.x**2 + self.y**2)
+        
+        # Если дрон находится рядом с целью (в радиусе 1 метра), останавливаемся
+        if distance_to_target < 1.0:
+            return None
+        
+        # Вычисляем угол к цели
+        target_angle = math.atan2(-self.y, -self.x)  # Отрицательные координаты, так как движемся к (0,0)
+        
+        # Вычисляем разность углов (направление поворота)
+        angle_diff = target_angle - self.angle
+
+        print(self.angle, target_angle, angle_diff)
+        
+        # Нормализуем разность углов в диапазон [-π, π]
+        angle_diff %= 2 * math.pi
+        if angle_diff >= math.pi:
+            angle_diff -= 2 * math.pi
+
+        # Возвращаем угол поворота (положительный - поворот влево, отрицательный - вправо)
+        return max(min(0.05, angle_diff / 2), -0.05)
 
     def reset_position(self, x: float = 0.0, y: float = 0.0, angle: float = 0.0):
         """Сбрасывает позицию и угол БПЛА"""
@@ -244,11 +268,17 @@ class AutoPilot(Pilot):
 
 
 class RandomPilot(Pilot):
+    counter: int
+    
     def __init__(self, velocity: float = 1):
-        pass
+        self.counter = 0
 
-    def act(self) -> float:
-        return 0.1
+    def act(self) -> float | None:
+        self.counter += 1
+        if self.counter > 40:
+            return None
+        
+        return 1 / (self.counter + 20)
 
 # def _test():
 #     randomPilot = RandomPilot()