fallback CMake + Armor PID

armor/include/armor_finder/armor_finder.h

@@ -120,6 +120,10 @@ private:
     vector<systime> time_seq;                           // 一个周期内的时间采样点
     vector<float> angle_seq;                            // 一个周期内的角度采样点
 
+    float yaw_rotation, pitch_rotation;//云台yaw轴和pitch轴应该转到的角度
+    float last_yaw, last_pitch;//PID中微分项
+    float sum_yaw, sum_pitch;//yaw和pitch的累计误差，即PID中积分项
+
     bool findLightBlobs(const cv::Mat &src, LightBlobs &light_blobs);
     bool findArmorBox(const cv::Mat &src, ArmorBox &box);
     bool matchArmorBoxes(const cv::Mat &src, const LightBlobs &light_blobs, ArmorBoxes &armor_boxes);

armor/src/armor_finder/send_target/send_target.cpp

@@ -22,6 +22,8 @@ static bool sendTarget(Serial &serial, double x, double y, double z, uint16_t sh
     fps += 1;
 #endif
 
+#define MINMAX(value, min, max) value = ((value) < (min)) ? (min) : ((value) > (max) ? (max) : (value))
+
     x_tmp = static_cast<short>(x * (32768 - 1) / 100);
     y_tmp = static_cast<short>(y * (32768 - 1) / 100);
     z_tmp = static_cast<short>(z * (32768 - 1) / 1000);
@@ -49,6 +51,24 @@ bool ArmorFinder::sendBoxPosition(uint16_t shoot_delay) {
     auto rect = target_box.rect;
     double dx = rect.x + rect.width / 2 - IMAGE_CENTER_X;
     double dy = rect.y + rect.height / 2 - IMAGE_CENTER_Y;
+
+    // PID
+    sum_yaw += dx;
+    sum_pitch += dy;
+    float yaw_I_component = YAW_AIM_KI * sum_yaw;
+    float pitch_I_component = PITCH_AIM_KI * sum_pitch;
+
+    double tmp_yaw = dx;
+    double tmp_pitch = dy;
+    dx = YAW_AIM_KP * dx + YAW_AIM_KI * sum_yaw +
+                   YAW_AIM_KD * (dx - last_yaw);
+    dy = PITCH_AIM_KP * dy + PITCH_AIM_KI * sum_pitch +
+                     PITCH_AIM_KD * (dy - last_pitch);
+
+    last_yaw = tmp_yaw;
+    last_pitch = tmp_pitch;
+    //
+
     double yaw = atan(dx / FOCUS_PIXAL) * 180 / PI;
     double pitch = atan(dy / FOCUS_PIXAL) * 180 / PI;
     double dist = DISTANCE_HEIGHT / rect.height;

others/include/camera/camera_wrapper.h

@@ -19,9 +19,9 @@ class CameraWrapper: public WrapperHead {
     friend void cameraCallback(CameraHandle hCamera, BYTE *pFrameBuffer, tSdkFrameHead* pFrameHead,PVOID pContext);
 private:
     const std::string name;
-    int mode;
+    //int mode;
 
-    bool init_done;
+    //bool init_done;
 
     unsigned char* rgb_buffer;
     int camera_cnts;
@@ -41,6 +41,9 @@ public:
     int gain;
     int exposure;
     
+    int mode;
+    bool init_done;
+
     CameraWrapper(int exposure, int gain, int camera_mode=1, const std::string &n="NULL");
     ~CameraWrapper() final;
 

others/src/additions.cpp

@@ -77,8 +77,12 @@ bool checkReconnect(bool is_camera_connect) {
     if (!is_camera_connect) {
         int curr_gain = ((CameraWrapper* )video)->gain;
         int curr_exposure = ((CameraWrapper* )video)->exposure;
+        int curr_mode = ((CameraWrapper* )video)->mode; // 获取原始模式
+        
         delete video;
-        video = new CameraWrapper(curr_exposure, curr_gain, 0/*, "armor"*/);
+        std::this_thread::sleep_for(std::chrono::milliseconds(500)); // 等待硬件释放
+        video = new CameraWrapper(curr_exposure, curr_gain, curr_mode/*, "armor"*/);
+        //video = new CameraWrapper(curr_exposure, curr_gain, 0/*, "armor"*/);
         is_camera_connect = video->init();
     }
     return is_camera_connect;

others/src/camera/camera_wrapper.cpp

@@ -38,6 +38,7 @@ bool CameraWrapper::init() {
     int camera_enumerate_device_status = CameraEnumerateDevice(camera_enum_list, &camera_cnts);
     if (camera_enumerate_device_status != CAMERA_STATUS_SUCCESS) {
         LOGE("CameraEnumerateDevice fail with %d!", camera_enumerate_device_status);
+        return false;
     }
     if (camera_cnts == 0) {
         LOGE("No camera device detected!");

others/src/options.cpp

@@ -14,7 +14,7 @@ bool show_light_blobs = false;
 bool show_origin = false;
 bool run_with_camera = true;
 bool save_video = false;
-bool wait_uart = true;
+bool wait_uart = false;
 bool save_labelled_boxes = false;
 bool show_process = false;
 bool show_energy = false;

xmake.lua

@@ -1,114 +0,0 @@
--- 设置项目信息
-set_project("SJTU-RM-CV")
-set_version("1.0.0")
-set_languages("c++17")
-
--- 设置构建模式
-set_rules("mode.release")
-set_optimize("fastest")
-
---导出构建指令
-add_rules("plugin.compile_commands.autoupdate", {outputdir = ".vscode"})
-
--- 定义宏
-add_defines('PATH=\"$(projectdir)\"')
-add_defines("Linux")
-
--- 检查是否存在 config.h
-if os.isfile("others/include/config/config.h") or os.isfile("others/include/config.h") then
-    add_defines("WITH_CONFIG")
-    print("Found config.h")
-end
-
--- 添加依赖
-add_requires("eigen", {system = false})
-add_requires("pthread", {system = true})
-add_requires("ffmpeg", {
-    system = false,
-    configs = {
-        iconv = true
-    }
-})
-add_requires("glib")
-add_requires("gdk-pixbuf", {system = false})
-
--- 目标配置
-target("run")
-    set_kind("binary")
-    
-    -- 添加源文件
-    add_files("main.cpp")
-    add_files("others/src/*.cpp")
-    add_files("others/src/camera/*.cpp")
-    add_files("energy/src/energy/*.cpp")
-    add_files("energy/src/energy/*/*.cpp")
-    add_files("armor/src/armor_finder/*.cpp")
-    add_files("armor/src/armor_finder/*/*.cpp")
-    add_files("armor/src/show_images/*.cpp")
-    
-    -- 添加头文件搜索路径
-    add_includedirs("others/include")
-    add_includedirs("others/include/camera")
-    add_includedirs("others/include/config")
-    add_includedirs("energy/include")
-    add_includedirs("energy/include/energy")
-    add_includedirs("armor/include")
-    add_includedirs("armor/include/armor_finder")
-    add_includedirs("armor/include/armor_finder/classifier")
-    add_includedirs("armor/include/show_images")
-    add_includedirs("/usr/local/include/opencv4")
-    
-    -- 添加依赖包
-    add_packages("pthread", "eigen", "opencv", "ffmpeg")
-    
-    -- 添加链接目录
-    add_linkdirs("others")
-    add_links("opencv")
-    
-    -- 根据平台链接相机 SDK
-    if is_plat("linux") then
-        add_links("MVSDK")
-        print("current platform: Linux")
-    elseif is_plat("windows") then
-        add_links("MVCAMSDK_X64")
-        print("current platform: Windows")
-    elseif is_plat("macosx") then
-        add_links("mvsdk")
-        print("current platform: Mac")
-    else
-        print("Unsupported platform")
-    end
-    
-    -- 设置目标目录
-    set_targetdir("$(builddir)")
-    
-    -- 添加编译选项
-    add_cxxflags("-O3")
-
--- 自定义任务：create-startup
-task("create-startup")
-    set_category("action")
-    on_run(function ()
-        os.exec("$(projectdir)/tools/create-startup.sh $(projectdir) $(builddir)")
-    end)
-    set_menu {
-        usage = "xmake create-startup",
-        description = "Create startup script",
-        options = {}
-    }
-
--- 自定义任务：train-cnn
-task("train-cnn")
-    set_category("action")
-    on_run(function ()
-        if os.host() == "linux" then
-            os.exec("gnome-terminal -- bash -c \"$(projectdir)/tools/TrainCNN/backward.py\"")
-        else
-            print("train-cnn only supported on Linux with gnome-terminal")
-        end
-    end)
-    set_menu {
-        usage = "xmake train-cnn",
-        description = "Train CNN model",
-        options = {}
-    }