From cb8f65aacf236f8270ec8cdc0ca33a7340514a14 Mon Sep 17 00:00:00 2001
From: raiot <raiot.lee@hotmail.com>
Date: Fri, 14 Jun 2024 23:27:10 +0800
Subject: [PATCH] feat: control algorithm

---
 agent.py   | 14 ++++++++++++++
 control.py | 38 ++++++++++++++++++++++++++++++++++++++
 2 files changed, 52 insertions(+)

diff --git a/agent.py b/agent.py
index 3349f83..da4b79a 100644
--- a/agent.py
+++ b/agent.py
@@ -6,10 +6,18 @@ import sys
 import time
 import logging
 import cv2
+from enum import Enum
 
 from control import ManualControl
 
 
+class Status(Enum):
+    INIT = 0
+    DETECT = 1
+    TRACK = 2
+    LANDING = 3
+
+
 class UAV:
     def __init__(self) -> None:
         self.tello = Tello()
@@ -26,6 +34,7 @@ class UAV:
 
         self.send_rc_control = False
         self.manual_control = ManualControl(self)
+        self.status = Status.INIT
 
         self.frame_queue = Queue(maxsize=1)  # 用于存放视频帧, 1用于避免帧堆积
     
@@ -51,6 +60,7 @@ class UAV:
         while True:
             # self.up_down_velocity(10)
             # self.send_rc_control = True
+            self.auto_control()
             self.update()
             self.video_display()
             time.sleep(0.05)
@@ -83,6 +93,10 @@ class UAV:
                 self.keep_recording = False
                 cv2.destroyAllWindows()
 
+    def auto_control(self):
+        if self.status == Status.INIT:
+            pass
+
         
             
 if __name__ == '__main__':
diff --git a/control.py b/control.py
index 4e8ddc0..c388d01 100644
--- a/control.py
+++ b/control.py
@@ -2,6 +2,34 @@ from pynput import keyboard
 import time
 
 
+TICKS_PER_SEC = 30
+
+
+class PIDController:
+    def __init__(self, kp, ki, kd, current_time):
+        self.kp = kp
+        self.ki = ki
+        self.kd = kd
+        self.previous_error = 0
+        self.integral = 0
+        self.last_time = current_time
+
+    def control(self, error, current_time):
+        current_time = current_time
+        delta_time = current_time - self.last_time
+        if delta_time == 0:
+            delta_time = 1 / TICKS_PER_SEC
+        derivative = (error - self.previous_error) / delta_time
+        self.integral += error * delta_time
+
+        output = self.kp * error + self.ki * self.integral + self.kd * derivative
+
+        self.previous_error = error
+        self.last_time = current_time
+
+        return output
+
+
 class ManualControl:
     def __init__(self, uav):
         self.uav = uav
@@ -45,6 +73,16 @@ class ManualControl:
         except AttributeError:
             pass
 
+
+class AutoControl:
+    def __init__(self, uav):
+        self.uav = uav
+        self.target = [0, 0, 0, 0]
+        self.pid = PIDController(1, 0, 0, time.time())
+
+    def detect(self):
+        pass
+
 class FakeUAV:
     def __init__(self) -> None:
         self.left_right_velocity = 0