Merge pull request 'PID 调整' (#1) from S-3-18 into master

Reviewed-on: #1

.vscode/settings.json

@@ -1,8 +1,8 @@
 {
         "clangd.arguments": [
-        "--compile-commands-dir=${workspaceFolder}/.vscode",
+        "--compile-commands-dir=${workspaceFolder}/build",
         "--completion-style=detailed",
-        "--query-driver=/usr/bin/g++,/usr/bin/gcc,/usr/bin/c++",
+        "--query-driver=/usr/bin/clang",
         "--header-insertion=never"
     ],
 }

CMakeLists.txt

@@ -8,6 +8,8 @@ SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DPATH=\"\\\"${PROJECT_SOURCE_DIR}\\\"\"
 SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -D${CMAKE_SYSTEM_NAME}")
 SET(BIN_NAME "run")
 
+SET(CMAKE_EXPORT_COMPILE_COMMANDS ON)
+
 
 FIND_FILE(CONFIG_FOUND "config.h" "others/include/config")
 if (CONFIG_FOUND)
@@ -24,7 +26,7 @@ IF(CCACHE_FOUND)
 ENDIF()
 
 FIND_PACKAGE(Eigen3 REQUIRED)
-FIND_PACKAGE(OpenCV 3 REQUIRED)
+FIND_PACKAGE(OpenCV 4 REQUIRED)
 FIND_PACKAGE(Threads)
 
 LINK_DIRECTORIES(${PROJECT_SOURCE_DIR}/others)

armor/include/armor_finder/armor_finder.h

@@ -20,8 +20,8 @@
 #define BOX_RED     ENEMY_RED
 #define BOX_BLUE    ENEMY_BLUE
 
-#define IMAGE_CENTER_X      (320)
+#define IMAGE_CENTER_X      (640)
-#define IMAGE_CENTER_Y      (240-20)
+#define IMAGE_CENTER_Y      (512-20)
 
 #define DISTANCE_HEIGHT_5MM (10700.0)     // 单位: cm*pixel
 #define DISTANCE_HEIGHT     DISTANCE_HEIGHT_5MM
@@ -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/armor.Config

@@ -5,17 +5,17 @@ resolution :
   image_size : 
   {
     iIndex = 1;
-    acDescription = "640X480 ROI";
+    acDescription = "1280X1024 ROI";
     uBinSumMode = 0;
     uBinAverageMode = 0;
     uSkipMode = 0;
     uResampleMask = 0;
     iHOffsetFOV = 56;
     iVOffsetFOV = 0;
-    iWidthFOV = 640;
+    iWidthFOV = 1280;
-    iHeightFOV = 480;
+    iHeightFOV = 1024;
-    iWidth = 640;
+    iWidth = 1280;
-    iHeight = 480;
+    iHeight = 1024;
     iWidthZoomHd = 0;
     iHeightZoomHd = 0;
     iWidthZoomSw = 0;

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;

others/include/config/setconfig.h

@@ -4,6 +4,7 @@
 #ifndef _SETCONFIG_H_
 #define _SETCONFIG_H_
 
+#define WITH_CONFIG
 
 #ifdef WITH_CONFIG
     #include <config/config.h>
@@ -49,13 +50,13 @@
     #define YAW_AIM_KI (0.01)
 #endif
 #ifndef PITCH_AIM_KD
-    #define PITCH_AIM_KD (0.4)
+    #define PITCH_AIM_KD (0)
 #endif
 #ifndef PITCH_AIM_KP
-    #define PITCH_AIM_KP (0.75)
+    #define PITCH_AIM_KP (0)
 #endif
 #ifndef PITCH_AIM_KI
-    #define PITCH_AIM_KI (0.01)
+    #define PITCH_AIM_KI (0)
 #endif
 
 #ifndef RED_COMPENSATE_YAW

others/include/constants.h

@@ -19,6 +19,6 @@
 
 #define FOCUS_PIXAL_8MM  (1488)
 #define FOCUS_PIXAL_5MM  (917)
-#define FOCUS_PIXAL      FOCUS_PIXAL_5MM
+#define FOCUS_PIXAL      FOCUS_PIXAL_8MM
 
 #endif /* _CONSTANTS_H */

others/src/additions.cpp

@@ -36,10 +36,10 @@ void uartReceive(Serial *pSerial) {
         pSerial->ReadData((uint8_t *) buffer, sizeof(mcu_data)+1);
         if (buffer[sizeof(mcu_data)] == '\n') {
             memcpy(&mcu_data, buffer, sizeof(mcu_data));
-//            LOGM("Get, state:%c, mark:%d!", mcu_data.state, (int) mcu_data.mark);
+           LOGM("Get, state:%c, mark:%d!", mcu_data.state, (int) mcu_data.mark);
-//            LOGM("Get yaw: %f, pitch: %f!", mcu_data.curr_yaw, mcu_data.curr_pitch);
+           LOGM("Get yaw: %f, pitch: %f!", mcu_data.curr_yaw, mcu_data.curr_pitch);
-//            LOGM("Get delta x: %d, delta y: %d!", mcu_data.delta_x, mcu_data.delta_y);
+            LOGM("Get delta x: %d, delta y: %d!", mcu_data.delta_x, mcu_data.delta_y);
-//            static int t = time(nullptr);
+ //           static int t = time(nullptr);
 //            static int cnt = 0;
 //            if(time(nullptr) > t){
 //                t = time(nullptr);
@@ -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;
@@ -103,14 +107,14 @@ void extract(cv::Mat &src) {//图像预处理，将视频切成640×480的大小
     if (src.empty()) return;
     float length = static_cast<float>(src.cols);
     float width = static_cast<float>(src.rows);
-    if (length / width > 640.0 / 480.0) {
+    if (length / width > 1280.0 / 1024.0) {
-        length *= 480.0 / width;
+        length *= 1024 / width;
-        resize(src, src, cv::Size(length, 480));
+        resize(src, src, cv::Size(length, 1024));
-        src = src(Rect((length - 640) / 2, 0, 640, 480));
+        src = src(Rect((length - 1280) / 2, 0, 1280, 1024));
     } else {
-        width *= 640.0 / length;
+        width *= 1280.0 / length;
-        resize(src, src, cv::Size(640, width));
+        resize(src, src, cv::Size(1280, width));
-        src = src(Rect(0, (width - 480) / 2, 640, 480));
+        src = src(Rect(0, (width - 1024) / 2, 1280, 1024));
     }
 }
 

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

@@ -19,7 +19,7 @@ bool save_labelled_boxes = false;
 bool show_process = false;
 bool show_energy = false;
 bool save_mark = false;
-bool show_info = false;
+bool show_info = true;
 bool run_by_frame = false;
 
 // 使用map保存所有选项及其描述和操作，加快查找速度。

others/src/serial.cpp

@@ -181,7 +181,7 @@ bool Serial::ReadData(unsigned char *buffer,  unsigned int length) {
 using namespace std;
 
 string get_uart_dev_name() {
-    FILE *ls = popen("ls /dev/ttyUSB* --color=never", "r");
+    FILE *ls = popen("ls /dev/ttyCH341USB* --color=never", "r");
     char name[20] = {0};
     fscanf(ls, "%s", name);
     return name;

xmake.lua

@@ -1,122 +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")
-
---- OpenCV 4.6.0 配置（启用 ffmpeg 和 contrib）
-add_requires("opencv", {
-    system = false,
-    configs = {
-        ffmpeg = false,
-        eigen = true,
-        png = true,
-        jpeg = true,
-        webp = false,
-        tiff = false,
-        quirc = false,
-        opengl = false,
-        protobuf = false,
-        bundled = true,
-        gtk = true
-    }
-})
-
--- 目标配置
-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_packages("pthread", "eigen", "opencv")
-    
-    -- 添加链接目录
-    add_linkdirs("others")
-    
-    -- 根据平台链接相机 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 = {}
-    }