From 3eb2d39942046d8fbe9e16d4726cd6e4a349b953 Mon Sep 17 00:00:00 2001 From: HelixCopex Date: Wed, 18 Mar 2026 17:06:04 +0800 Subject: [PATCH] fallback CMake + Armor PID --- armor/include/armor_finder/armor_finder.h | 4 + .../armor_finder/send_target/send_target.cpp | 20 +++ others/include/camera/camera_wrapper.h | 7 +- others/src/additions.cpp | 6 +- others/src/camera/camera_wrapper.cpp | 1 + others/src/options.cpp | 2 +- xmake.lua | 114 ------------------ 7 files changed, 36 insertions(+), 118 deletions(-) delete mode 100644 xmake.lua diff --git a/armor/include/armor_finder/armor_finder.h b/armor/include/armor_finder/armor_finder.h index a0df0e1..4c690b6 100644 --- a/armor/include/armor_finder/armor_finder.h +++ b/armor/include/armor_finder/armor_finder.h @@ -120,6 +120,10 @@ private: vector time_seq; // 一个周期内的时间采样点 vector 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); diff --git a/armor/src/armor_finder/send_target/send_target.cpp b/armor/src/armor_finder/send_target/send_target.cpp index 8e45ad0..0a77bfd 100644 --- a/armor/src/armor_finder/send_target/send_target.cpp +++ b/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(x * (32768 - 1) / 100); y_tmp = static_cast(y * (32768 - 1) / 100); z_tmp = static_cast(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; diff --git a/others/include/camera/camera_wrapper.h b/others/include/camera/camera_wrapper.h index 3fe58dc..93826ee 100644 --- a/others/include/camera/camera_wrapper.h +++ b/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; @@ -40,6 +40,9 @@ private: 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; diff --git a/others/src/additions.cpp b/others/src/additions.cpp index a46d58d..c76a115 100644 --- a/others/src/additions.cpp +++ b/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; diff --git a/others/src/camera/camera_wrapper.cpp b/others/src/camera/camera_wrapper.cpp index 6c19fe5..9a8cd1d 100644 --- a/others/src/camera/camera_wrapper.cpp +++ b/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!"); diff --git a/others/src/options.cpp b/others/src/options.cpp index acb507d..bf17c43 100644 --- a/others/src/options.cpp +++ b/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; diff --git a/xmake.lua b/xmake.lua deleted file mode 100644 index 71a6679..0000000 --- a/xmake.lua +++ /dev/null @@ -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 = {} - } \ No newline at end of file