有vofa验证

.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_Y      (240-20)
+#define IMAGE_CENTER_X      (640)
+#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/anti_top/anti_top.cpp

@@ -38,6 +38,14 @@ void ArmorFinder::antiTop() {
     // 通过两次装甲角度为零的时间差计算陀螺旋转周期
     // 根据旋转周期计算下一次装甲出现在角度为零的时间点
     if (getPointLength(last_box.getCenter() - target_box.getCenter()) > last_box.rect.height * 1.5) {
+        /* PID相关控制 */
+        sum_yaw = sum_pitch = 0;
+        double dx = target_box.rect.x + target_box.rect.width / 2 - IMAGE_CENTER_X;
+        double dy = target_box.rect.y + target_box.rect.height / 2 - IMAGE_CENTER_Y;
+        double yaw = dx;
+        double pitch = dy;
+        /* PID相关控制 */
+        
         auto front_time = getFrontTime(time_seq, angle_seq);
         auto once_periodms = getTimeIntervalms(front_time, last_front_time);
 //        if (abs(once_periodms - top_periodms[-1]) > 50) {

armor/src/armor_finder/armor_finder.cpp

@@ -54,7 +54,11 @@ ArmorFinder::ArmorFinder(uint8_t &color, Serial &u, const string &paras_folder,
         anti_switch_cnt(0),
         classifier(paras_folder),
         contour_area(0),
-        tracking_cnt(0) {
+        tracking_cnt(0),
+        last_yaw(0), 
+        last_pitch(0), 
+        sum_yaw(0), 
+        sum_pitch(0) {
 }
 
 void ArmorFinder::run(cv::Mat &src) {
@@ -75,6 +79,9 @@ void ArmorFinder::run(cv::Mat &src) {
                     tracker->init(src, target_box.rect);
                     state = TRACKING_STATE;
                     tracking_cnt = 0;
+                    last_yaw = target_box.rect.x + target_box.rect.width / 2 - IMAGE_CENTER_X;
+                    last_pitch = target_box.rect.y + target_box.rect.height / 2 - IMAGE_CENTER_Y;
+                    sum_yaw = sum_pitch = 0;
                     LOGM(STR_CTR(WORD_LIGHT_CYAN, "into track"));
                 }
             }
@@ -82,6 +89,8 @@ void ArmorFinder::run(cv::Mat &src) {
         case TRACKING_STATE:
             if (!stateTrackingTarget(src) || ++tracking_cnt > 100) {    // 最多追踪100帧图像
                 state = SEARCHING_STATE;
+                last_yaw = last_pitch = 0;
+                sum_yaw = sum_pitch = 0;
                 LOGM(STR_CTR(WORD_LIGHT_YELLOW, "into search!"));
             }
             break;
@@ -93,6 +102,11 @@ end:
     if(is_anti_top) { // 判断当前是否为反陀螺模式
         antiTop();
     }else if(target_box.rect != cv::Rect2d()) {
+        if(last_box.rect == cv::Rect2d()) { // 如果上一帧没有目标装甲板
+            sum_yaw = sum_pitch = 0;
+            last_yaw = target_box.rect.x + target_box.rect.width / 2 - IMAGE_CENTER_X;
+            last_pitch = target_box.rect.y + target_box.rect.height / 2 - IMAGE_CENTER_Y;
+        }
         anti_top_cnt = 0;
         time_seq.clear();
         angle_seq.clear();

armor/src/armor_finder/send_target/send_target.cpp

@@ -7,6 +7,7 @@
 #include <log.h>
 
 static bool sendTarget(Serial &serial, double x, double y, double z, uint16_t shoot_delay) {
+    
     static short x_tmp, y_tmp, z_tmp;
     uint8_t buff[10];
 
@@ -22,10 +23,13 @@ static bool sendTarget(Serial &serial, double x, double y, double z, uint16_t sh
     fps += 1;
 #endif
 
-    x_tmp = static_cast<short>(x * (32768 - 1) / 100);
+#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);
     
+
     buff[0] = 's';
     buff[1] = static_cast<char>((x_tmp >> 8) & 0xFF);
     buff[2] = static_cast<char>((x_tmp >> 0) & 0xFF);
@@ -39,16 +43,48 @@ static bool sendTarget(Serial &serial, double x, double y, double z, uint16_t sh
     //if(buff[7]<<8 | buff[8])
     //   cout << (buff[7]<<8 | buff[8]) << endl;
     return serial.WriteData(buff, sizeof(buff));
+  
+    
+    // Vofa串口验证
+    
+    //char buff[128];
+    //int len = (buff, "s %lf %lf %lf %d e", x, y, z, shoot_delay);
+    //return serial.WriteData((unsigned char *)buff, len);
+    
+    
+    
 }
 
 bool ArmorFinder::sendBoxPosition(uint16_t shoot_delay) {
     if (target_box.rect == cv::Rect2d()) return false;
-    if (shoot_delay) {
+    /*if (shoot_delay) {
         LOGM(STR_CTR(WORD_BLUE, "next box %dms"), 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
+    #define MAX_YAW 100
+    #define MAX_PITCH 80
+    sum_yaw += dx;
+    sum_pitch += dy;
+    sum_yaw =(sum_yaw > MAX_YAW) ? MAX_YAW : (sum_yaw < -MAX_YAW ? -MAX_YAW : sum_yaw);
+    sum_pitch =(sum_pitch > MAX_PITCH) ? MAX_PITCH : (sum_pitch < -MAX_PITCH ? -MAX_PITCH : sum_pitch);
+    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;

main.cpp

@@ -44,7 +44,9 @@ WrapperHead *video = nullptr;    // 云台摄像头视频源
 Serial serial(115200);                  // 串口对象
 uint8_t last_state = ARMOR_STATE;     // 上次状态，用于初始化
 // 自瞄主程序对象
-ArmorFinder armor_finder(mcu_data.enemy_color, serial, PROJECT_DIR"/tools/para/", mcu_data.anti_top);
+uint8_t enemy_color = ENEMY_BLUE;//RED ro BLUE
+uint8_t forced_anti_top = 1;
+ArmorFinder armor_finder(enemy_color, serial, PROJECT_DIR"/tools/para/", forced_anti_top);
 // 能量机关主程序对象
 Energy energy(serial, mcu_data.enemy_color);
 

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;
-    iHeightFOV = 480;
-    iWidth = 640;
-    iHeight = 480;
+    iWidthFOV = 1280;
+    iHeightFOV = 1024;
+    iWidth = 1280;
+    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;
@@ -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/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,9 +36,9 @@ 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 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, 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 delta x: %d, delta y: %d!", mcu_data.delta_x, mcu_data.delta_y);
  //           static int t = time(nullptr);
 //            static int cnt = 0;
 //            if(time(nullptr) > t){
@@ -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) {
-        length *= 480.0 / width;
-        resize(src, src, cv::Size(length, 480));
-        src = src(Rect((length - 640) / 2, 0, 640, 480));
+    if (length / width > 1280.0 / 1024.0) {
+        length *= 1024 / width;
+        resize(src, src, cv::Size(length, 1024));
+        src = src(Rect((length - 1280) / 2, 0, 1280, 1024));
     } else {
-        width *= 640.0 / length;
-        resize(src, src, cv::Size(640, width));
-        src = src(Rect(0, (width - 480) / 2, 640, 480));
+        width *= 1280.0 / length;
+        resize(src, src, cv::Size(1280, width));
+        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 = {}
-    }