修改发包逻辑

Reviewed-on: VSAG/vision_sjtu_19#1

.gitignore

@@ -1,6 +1,9 @@
 cmake-build-debug
 build
 .idea
+.xmake
+.vscode/.cache
+.vscode/compile_commands.json
 Mark
 armor_box_photo
 tools/TrainCNN/.idea

.vscode/settings.json

@@ -0,0 +1,8 @@
+{
+        "clangd.arguments": [
+        "--compile-commands-dir=${workspaceFolder}/build",
+        "--completion-style=detailed",
+        "--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)

README.md

@@ -13,6 +13,10 @@
 
 **如有BUG或者想交流的朋友欢迎积极联系我们**
 
+**分享部分比赛时摄像头录制的视频:**
+
+链接: https://pan.baidu.com/s/1LwxEpeYYblX3cSzb59MTVg 提取码: 84ju 复制这段内容后打开百度网盘手机App，操作更方便哦
+
 ---
 
 运行效果：自瞄帧率120（摄像头最大帧率）,识别距离根据环境不同大约8米左右(5mm焦距镜头)。
@@ -26,6 +30,8 @@
 | ---------------------------------------------------- | -------------------------------------------- | ------------------------------------------------------------ | ---------------------------------------------------------- |
 | IntelNUC<br />MindVision工业相机×１<br />USB转TTL×１ | Ubuntu18.04<br />Ubuntu16.04<br />Ｗindows10 | OpenCV3.4.5<br />OpenCV_contrib3.4.5<br />Eigen3<br />MindVision相机驱动 | Ubuntu18/16 : cmake3+gcc7+g++7 <br />Win10 : cmake3+VS2019 |
 
+MindVision相机型号：MV-UBS31GC
+
 **关于Windows环境下的运行支持，仅保证程序可以编译运行。对与部分辅助功能，如生成自启动脚本则不支持。**
 
 **实际装载在步兵和哨兵上的运行环境为Ubuntu18.04。**
@@ -62,6 +68,8 @@ sudo ./run
 
 ```./run --help```可以查看所有命令行参数及其作用。所有调试选项都集成到了命令行参数中。
 
+**不使用任何参数直接运行将没有任何图像显示。**
+
 需要调参的部分：主要需要根据车辆情况而调参的参数存放在others/include/config/setconfig.h中
 
 ### 3.工作条件

armor/include/armor_finder/armor_finder.h

@@ -9,7 +9,7 @@
 #include <systime.h>
 #include <constants.h>
 #include <opencv2/core.hpp>
-#include <opencv2/tracking.hpp>
+#include <opencv2/tracking/tracking.hpp>
 #include <serial.h>
 #include <armor_finder/classifier/classifier.h>
 #include <additions.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/armor_finder.cpp

@@ -67,7 +67,7 @@ void ArmorFinder::run(cv::Mat &src) {
                 if ((target_box.rect & cv::Rect2d(0, 0, 640, 480)) == target_box.rect) { // 判断装甲板区域是否脱离图像区域
                     if (!classifier) {                                          /* 如果分类器不可用 */
                         cv::Mat roi = src(target_box.rect).clone(), roi_gray;  /* 就使用装甲区域亮点数判断是否跟丢 */
-                        cv::cvtColor(roi, roi_gray, CV_RGB2GRAY);
+                        cv::cvtColor(roi, roi_gray, cv::COLOR_RGB2GRAY);
                         cv::threshold(roi_gray, roi_gray, 180, 255, cv::THRESH_BINARY);
                         contour_area = cv::countNonZero(roi_gray);
                     }

armor/src/armor_finder/find/find_light_blobs.cpp

@@ -83,11 +83,11 @@ bool ArmorFinder::findLightBlobs(const cv::Mat &src, LightBlobs &light_blobs) {
     }else{
         light_threshold = 200;
     }
-    cv::threshold(color_channel, src_bin_light, light_threshold, 255, CV_THRESH_BINARY); // 二值化对应通道
+    cv::threshold(color_channel, src_bin_light, light_threshold, 255, cv::THRESH_BINARY); // 二值化对应通道
     if (src_bin_light.empty()) return false;
     imagePreProcess(src_bin_light);                                  // 开闭运算
 
-    cv::threshold(color_channel, src_bin_dim, 140, 255, CV_THRESH_BINARY); // 二值化对应通道
+    cv::threshold(color_channel, src_bin_dim, 140, 255, cv::THRESH_BINARY); // 二值化对应通道
     if (src_bin_dim.empty()) return false;
     imagePreProcess(src_bin_dim);                                  // 开闭运算
 
@@ -100,8 +100,8 @@ bool ArmorFinder::findLightBlobs(const cv::Mat &src, LightBlobs &light_blobs) {
     std::vector<std::vector<cv::Point>> light_contours_light, light_contours_dim;
     LightBlobs light_blobs_light, light_blobs_dim;
     std::vector<cv::Vec4i> hierarchy_light, hierarchy_dim;
-    cv::findContours(src_bin_light, light_contours_light, hierarchy_light, CV_RETR_CCOMP, CV_CHAIN_APPROX_NONE);
-    cv::findContours(src_bin_dim, light_contours_dim, hierarchy_dim, CV_RETR_CCOMP, CV_CHAIN_APPROX_NONE);
+    cv::findContours(src_bin_light, light_contours_light, hierarchy_light, cv::RETR_CCOMP, cv::CHAIN_APPROX_NONE);
+    cv::findContours(src_bin_dim, light_contours_dim, hierarchy_dim, cv::RETR_CCOMP, cv::CHAIN_APPROX_NONE);
     for (int i = 0; i < light_contours_light.size(); i++) {
         if (hierarchy_light[i][2] == -1) {
             cv::RotatedRect rect = cv::minAreaRect(light_contours_light[i]);

armor/src/armor_finder/send_target/send_target.cpp

@@ -6,9 +6,35 @@
 #include <config/setconfig.h>
 #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];
+// amadeus_26 协议: 15字节帧
+// | SOF(0xBB 0x77) | x_move(2B) | y_move(2B) | yaw(2B) | pitch(2B) | feed(2B) | key(1B) | crc8(1B) | EOF(0xEE) |
+constexpr uint8_t FRAME_HEADER_1 = 0xBB;
+constexpr uint8_t FRAME_HEADER_2 = 0x77;
+constexpr uint8_t FRAME_TAIL = 0xEE;
+constexpr int FRAME_LENGTH = 15;
+
+static uint8_t calculateCRC8(const uint8_t *data, size_t len) {
+    uint8_t crc = 0xFF;
+    for (size_t i = 0; i < len; i++) {
+        crc ^= data[i];
+        for (int j = 0; j < 8; j++) {
+            if (crc & 0x80) {
+                crc = (crc << 1) ^ 0x31;
+            } else {
+                crc <<= 1;
+            }
+        }
+    }
+    return crc;
+}
+
+static bool sendTarget(Serial &serial, double yaw, double pitch, double dist, uint16_t shoot_delay) {
+    int16_t x_move = 0;      // 平动左右 [-660, 660]
+    int16_t y_move = 0;      // 平动前后 [-660, 660]
+    int16_t yaw_val = 0;     // 云台偏航 [-660, 660]
+    int16_t pitch_val = 0;   // 云台俯仰 [-660, 660]
+    int16_t feed = 0;        // 拨弹轮 [-660, 660]
+    uint8_t key = 0;         // 按键 [0, 15]
 
 #ifdef WITH_COUNT_FPS
     static time_t last_time = time(nullptr);
@@ -16,29 +42,64 @@ static bool sendTarget(Serial &serial, double x, double y, double z, uint16_t sh
     time_t t = time(nullptr);
     if (last_time != t) {
         last_time = t;
-        cout << "Armor: fps:" << fps << ", (" << x << "," << y << "," << z << ")" << endl;
+        cout << "Armor: fps:" << fps << ", (" << yaw << "," << pitch << "," << dist << ")" << endl;
         fps = 0;
     }
     fps += 1;
 #endif
 
-    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);
+#define MINMAX(value, min, max) value = ((value) < (min)) ? (min) : ((value) > (max) ? (max) : (value))
 
-    buff[0] = 's';
-    buff[1] = static_cast<char>((x_tmp >> 8) & 0xFF);
-    buff[2] = static_cast<char>((x_tmp >> 0) & 0xFF);
-    buff[3] = static_cast<char>((y_tmp >> 8) & 0xFF);
-    buff[4] = static_cast<char>((y_tmp >> 0) & 0xFF);
-    buff[5] = static_cast<char>((z_tmp >> 8) & 0xFF);
-    buff[6] = static_cast<char>((z_tmp >> 0) & 0xFF);
-    buff[7] = static_cast<char>((shoot_delay >> 8) & 0xFF);
-    buff[8] = static_cast<char>((shoot_delay >> 0) & 0xFF);
-    buff[9] = 'e';
-//    if(buff[7]<<8 | buff[8])
-//        cout << (buff[7]<<8 | buff[8]) << endl;
-    return serial.WriteData(buff, sizeof(buff));
+    // 将角度转换为协议范围 [-660, 660]
+    yaw_val = static_cast<int16_t>(yaw * 10);     // 角度放大10倍
+    pitch_val = static_cast<int16_t>(pitch * 10);
+    MINMAX(yaw_val, -660, 660);
+    MINMAX(pitch_val, -660, 660);
+
+    // 如果需要发弹，设置 feed 值
+    if (shoot_delay > 0) {
+        feed = 660;  // 开火
+    }
+
+    uint8_t frame[FRAME_LENGTH];
+    int idx = 0;
+
+    // 帧头
+    frame[idx++] = FRAME_HEADER_1;
+    frame[idx++] = FRAME_HEADER_2;
+
+    // 平动左右 (2 bytes, int16, 小端序)
+    frame[idx++] = x_move & 0xFF;
+    frame[idx++] = (x_move >> 8) & 0xFF;
+
+    // 平动前后 (2 bytes, 小端序)
+    frame[idx++] = y_move & 0xFF;
+    frame[idx++] = (y_move >> 8) & 0xFF;
+
+    // 云台偏航 (2 bytes, 小端序)
+    frame[idx++] = yaw_val & 0xFF;
+    frame[idx++] = (yaw_val >> 8) & 0xFF;
+
+    // 云台俯仰 (2 bytes, 小端序)
+    frame[idx++] = pitch_val & 0xFF;
+    frame[idx++] = (pitch_val >> 8) & 0xFF;
+
+    // 拨弹轮 (2 bytes, 小端序)
+    frame[idx++] = feed & 0xFF;
+    frame[idx++] = (feed >> 8) & 0xFF;
+
+    // 按键 (1 byte)
+    frame[idx++] = key;
+
+    // CRC8 (数据区: frame[2]到frame[12], 共11字节)
+    // 当前固定为0xCC，如需启用CRC校验，取消下面注释
+    // frame[idx++] = calculateCRC8(frame + 2, 11);
+    frame[idx++] = 0xCC;
+
+    // 帧尾
+    frame[idx++] = FRAME_TAIL;
+
+    return serial.WriteData(frame, sizeof(frame));
 }
 
 bool ArmorFinder::sendBoxPosition(uint16_t shoot_delay) {
@@ -49,6 +110,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;

armor/src/armor_finder/tracking_state/tracking_state.cpp

@@ -8,13 +8,13 @@
 #include <show_images/show_images.h>
 
 bool ArmorFinder::stateTrackingTarget(cv::Mat &src) {
-    auto pos = target_box.rect;
+    cv::Rect pos(target_box.rect);
     if(!tracker->update(src, pos)){ // 使用KCFTracker进行追踪
         target_box = ArmorBox();
         LOGW("Track fail!");
         return false;
     }
-    if((pos & cv::Rect2d(0, 0, 640, 480)) != pos){
+    if((cv::Rect2d(pos) & cv::Rect2d(0, 0, 640, 480)) != cv::Rect2d(pos)){
         target_box = ArmorBox();
         LOGW("Track out range!");
         return false;
@@ -52,7 +52,7 @@ bool ArmorFinder::stateTrackingTarget(cv::Mat &src) {
             }
         }else{ //　分类器不可用，使用常规方法判断
             cv::Mat roi_gray;
-            cv::cvtColor(roi, roi_gray, CV_RGB2GRAY);
+            cv::cvtColor(roi, roi_gray, cv::COLOR_RGB2GRAY);
             cv::threshold(roi_gray, roi_gray, 180, 255, cv::THRESH_BINARY);
             contour_area = cv::countNonZero(roi_gray);
             if(abs(cv::countNonZero(roi_gray) - contour_area) > contour_area * 0.3){

energy/include/energy/energy.h

@@ -35,7 +35,7 @@ public:
     Serial &serial;//串口
     void setEnergyInit();//设置能量机关初始化
     void sendEnergy();//发送能量机关数据
-    void sendTarget(Serial& serial, float x, float y, float z, uint16_t u);//发送数据
+    void sendTarget(Serial& serial, float yaw, float pitch, int16_t feed, uint8_t key);//发送数据 (amadeus_26 协议)
 
 
 private:

energy/src/energy/find/energy_finder.cpp

@@ -21,12 +21,12 @@ int Energy::findFans(const cv::Mat &src) {
     static Mat src_bin;
     src_bin = src.clone();
     if (src.type() == CV_8UC3) {
-        cvtColor(src_bin, src_bin, CV_BGR2GRAY);//若读取三通道视频文件，需转换为单通道
+        cvtColor(src_bin, src_bin, cv::COLOR_BGR2GRAY);//若读取三通道视频文件，需转换为单通道
     }
     std::vector<vector<Point> > fan_contours;
     FanStruct(src_bin);//图像膨胀，防止图像断开并更方便寻找
     if (show_process)imshow("fan struct", src_bin);
-    findContours(src_bin, fan_contours, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_NONE);
+    findContours(src_bin, fan_contours, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_NONE);
 
     for (auto &fan_contour : fan_contours) {
         if (!isValidFanContour(src_bin, fan_contour)) {
@@ -54,16 +54,16 @@ int Energy::findArmors(const cv::Mat &src) {
     static Mat src_bin;
     src_bin = src.clone();
     if (src.type() == CV_8UC3) {
-        cvtColor(src_bin, src_bin, CV_BGR2GRAY);//若读取三通道视频文件，需转换为单通道
+        cvtColor(src_bin, src_bin, cv::COLOR_BGR2GRAY);//若读取三通道视频文件，需转换为单通道
     }
     std::vector<vector<Point> > armor_contours;
     std::vector<vector<Point> > armor_contours_external;//用总轮廓减去外轮廓，只保留内轮廓，除去流动条的影响。
 
     ArmorStruct(src_bin);//图像膨胀，防止图像断开并更方便寻找
-    findContours(src_bin, armor_contours, CV_RETR_LIST, CV_CHAIN_APPROX_NONE);
+    findContours(src_bin, armor_contours, cv::RETR_LIST, cv::CHAIN_APPROX_NONE);
     if (show_process)imshow("armor struct", src_bin);
 
-    findContours(src_bin, armor_contours_external, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_NONE);
+    findContours(src_bin, armor_contours_external, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_NONE);
     for (int i = 0; i < armor_contours_external.size(); i++)//去除外轮廓
     {
         unsigned long external_contour_size = armor_contours_external[i].size();
@@ -103,12 +103,12 @@ bool Energy::findCenterR(const cv::Mat &src) {
     static Mat src_bin;
     src_bin = src.clone();
     if (src.type() == CV_8UC3) {
-        cvtColor(src_bin, src_bin, CV_BGR2GRAY);
+        cvtColor(src_bin, src_bin, cv::COLOR_BGR2GRAY);
     }
     std::vector<vector<Point> > center_R_contours;
     CenterRStruct(src_bin);
     if (show_process)imshow("R struct", src_bin);
-    findContours(src_bin, center_R_contours, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_NONE);
+    findContours(src_bin, center_R_contours, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_NONE);
     for (auto &center_R_contour : center_R_contours) {
         if (!isValidCenterRContour(center_R_contour)) {
             continue;
@@ -139,13 +139,13 @@ bool Energy::findFlowStripFan(const cv::Mat &src) {
     src_bin = src.clone();
     src_copy = src.clone();
     if (src.type() == CV_8UC3) {
-        cvtColor(src_bin, src_bin, CV_BGR2GRAY);//若读取三通道视频文件，需转换为单通道
+        cvtColor(src_bin, src_bin, cv::COLOR_BGR2GRAY);//若读取三通道视频文件，需转换为单通道
     }
     std::vector<vector<Point> > flow_strip_fan_contours;
     FlowStripFanStruct(src_bin);//图像膨胀，防止图像断开并更方便寻找
     if (show_process)imshow("flow strip fan struct", src_bin);
 
-    findContours(src_bin, flow_strip_fan_contours, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_NONE);
+    findContours(src_bin, flow_strip_fan_contours, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_NONE);
     std::vector<cv::RotatedRect> candidate_flow_strip_fans;
 
     for (auto &flow_strip_fan_contour : flow_strip_fan_contours) {
@@ -175,7 +175,7 @@ bool Energy::findFlowStrip(const cv::Mat &src) {
 
     if (src_bin.type() == CV_8UC1) // 黑白图像
     {
-        cvtColor(src_bin, src_bin, COLOR_GRAY2RGB);
+        cvtColor(src_bin, src_bin, cv::COLOR_GRAY2RGB);
 
     }
     std::vector<cv::RotatedRect> candidate_target_armors = target_armors;
@@ -189,13 +189,13 @@ bool Energy::findFlowStrip(const cv::Mat &src) {
         }
     }
 
-    cvtColor(src_bin, src_bin, CV_BGR2GRAY);//若读取三通道视频文件，需转换为单通道
+    cvtColor(src_bin, src_bin, cv::COLOR_BGR2GRAY);//若读取三通道视频文件，需转换为单通道
 
     FlowStripStruct(src_bin);//图像膨胀，防止图像断开并更方便寻找
     if (show_process)imshow("flow strip struct", src_bin);
 
     std::vector<vector<Point> > flow_strip_contours;
-    findContours(src_bin, flow_strip_contours, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_NONE);
+    findContours(src_bin, flow_strip_contours, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_NONE);
 
     for (auto candidate_flow_strip_fan: flow_strip_fans) {
         for (auto &flow_strip_contour : flow_strip_contours) {
@@ -261,7 +261,7 @@ bool Energy::findFlowStripWeak(const cv::Mat &src) {
 
     if (src_bin.type() == CV_8UC1) // 黑白图像
     {
-        cvtColor(src_bin, src_bin, COLOR_GRAY2RGB);
+        cvtColor(src_bin, src_bin, cv::COLOR_GRAY2RGB);
 
     }
     std::vector<cv::RotatedRect> candidate_armors = armors;
@@ -275,13 +275,13 @@ bool Energy::findFlowStripWeak(const cv::Mat &src) {
         }
     }
 
-    cvtColor(src_bin, src_bin, CV_BGR2GRAY);//若读取三通道视频文件，需转换为单通道
+    cvtColor(src_bin, src_bin, cv::COLOR_BGR2GRAY);//若读取三通道视频文件，需转换为单通道
 
     FlowStripStruct(src_bin);//图像膨胀，防止图像断开并更方便寻找
     if (show_process)imshow("weak struct", src_bin);
 
     std::vector<vector<Point> > flow_strip_contours;
-    findContours(src_bin, flow_strip_contours, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_NONE);
+    findContours(src_bin, flow_strip_contours, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_NONE);
 
     for (auto &flow_strip_contour : flow_strip_contours) {
         if (!isValidFlowStripContour(flow_strip_contour)) {
@@ -336,5 +336,3 @@ bool Energy::findCenterROI(const cv::Mat &src) {
     return true;
 
 }
-
-

energy/src/energy/send/send.cpp

@@ -8,6 +8,13 @@
 
 using namespace std;
 
+// amadeus_26 协议: 15字节帧
+// | SOF(0xBB 0x77) | x_move(2B) | y_move(2B) | yaw(2B) | pitch(2B) | feed(2B) | key(1B) | crc8(1B) | EOF(0xEE) |
+constexpr uint8_t FRAME_HEADER_1 = 0xBB;
+constexpr uint8_t FRAME_HEADER_2 = 0x77;
+constexpr uint8_t FRAME_TAIL = 0xEE;
+constexpr int FRAME_LENGTH = 15;
+
 #define MINMAX(value, min, max) value = ((value) < (min)) ? (min) : ((value) > (max) ? (max) : (value))
 
 //----------------------------------------------------------------------------------------------------------------------
@@ -31,23 +38,32 @@ void Energy::sendEnergy() {
     last_yaw = tmp_yaw;
     last_pitch = tmp_pitch;
 
-    if (change_target) {
-        sendTarget(serial, yaw_rotation, pitch_rotation, 3, 0);//表示目标切换
-    } else if (is_guessing) {
-        sendTarget(serial, yaw_rotation, pitch_rotation, 4, 0);//表示猜测模式
-    } else {
-        sendTarget(serial, yaw_rotation, pitch_rotation, shoot, 0);//跟随或发弹
+    // 能量机关模式通过 key 字段区分
+    // 模式编码: 0=普通跟随, 3=目标切换, 4=猜测模式
+    uint8_t mode_key = 0;
+    int16_t feed = 0;
+    if (shoot) {
+        feed = 660;  // 开火
     }
 
+    if (change_target) {
+        mode_key = 3;  // 目标切换
+    } else if (is_guessing) {
+        mode_key = 4;  // 猜测模式
+    }
+
+    sendTarget(serial, yaw_rotation, pitch_rotation, feed, mode_key);
 }
 
 
 //----------------------------------------------------------------------------------------------------------------------
-// 此函数用于发送数据给主控板
+// 此函数用于发送数据给主控板 (amadeus_26 协议)
 // ---------------------------------------------------------------------------------------------------------------------
-void Energy::sendTarget(Serial &serial, float x, float y, float z, uint16_t u) {
-    short x_tmp, y_tmp, z_tmp;
-    uint8_t buff[10];
+void Energy::sendTarget(Serial &serial, float yaw, float pitch, int16_t feed, uint8_t key) {
+    int16_t x_move = 0;      // 平动左右 [-660, 660]
+    int16_t y_move = 0;      // 平动前后 [-660, 660]
+    int16_t yaw_val = 0;     // 云台偏航 [-660, 660]
+    int16_t pitch_val = 0;   // 云台俯仰 [-660, 660]
 
 #ifdef WITH_COUNT_FPS
     static auto last_time = time(nullptr);
@@ -55,27 +71,57 @@ void Energy::sendTarget(Serial &serial, float x, float y, float z, uint16_t u) {
     time_t t = time(nullptr);
     if (last_time != t) {
         last_time = t;
-        cout << "Energy: fps:" << fps << ", (" << x << "," << y << "," << z << "," << u << ")" << endl;
+        cout << "Energy: fps:" << fps << ", (" << yaw << "," << pitch << ", feed:" << feed << ", key:" << (int)key << ")" << endl;
         curr_fps = fps;
         fps = 0;
     }
     fps += 1;
 #endif
 
-    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) / 100);
-    buff[0] = 's';
-    buff[1] = static_cast<char>((x_tmp >> 8) & 0xFF);
-    buff[2] = static_cast<char>((x_tmp >> 0) & 0xFF);
-    buff[3] = static_cast<char>((y_tmp >> 8) & 0xFF);
-    buff[4] = static_cast<char>((y_tmp >> 0) & 0xFF);
-    buff[5] = static_cast<char>((z_tmp >> 8) & 0xFF);
-    buff[6] = static_cast<char>((z_tmp >> 0) & 0xFF);
-    buff[7] = static_cast<char>((u >> 8) & 0xFF);
-    buff[8] = static_cast<char>((u >> 0) & 0xFF);
-    buff[9] = 'e';
-    serial.WriteData(buff, sizeof(buff));
+    // 将角度转换为协议范围 [-660, 660]
+    yaw_val = static_cast<int16_t>(yaw * 10);     // 角度放大10倍
+    pitch_val = static_cast<int16_t>(pitch * 10);
+    MINMAX(yaw_val, -660, 660);
+    MINMAX(pitch_val, -660, 660);
+
+    uint8_t frame[FRAME_LENGTH];
+    int idx = 0;
+
+    // 帧头
+    frame[idx++] = FRAME_HEADER_1;
+    frame[idx++] = FRAME_HEADER_2;
+
+    // 平动左右 (2 bytes, int16, 小端序)
+    frame[idx++] = x_move & 0xFF;
+    frame[idx++] = (x_move >> 8) & 0xFF;
+
+    // 平动前后 (2 bytes, 小端序)
+    frame[idx++] = y_move & 0xFF;
+    frame[idx++] = (y_move >> 8) & 0xFF;
+
+    // 云台偏航 (2 bytes, 小端序)
+    frame[idx++] = yaw_val & 0xFF;
+    frame[idx++] = (yaw_val >> 8) & 0xFF;
+
+    // 云台俯仰 (2 bytes, 小端序)
+    frame[idx++] = pitch_val & 0xFF;
+    frame[idx++] = (pitch_val >> 8) & 0xFF;
+
+    // 拨弹轮 (2 bytes, 小端序)
+    frame[idx++] = feed & 0xFF;
+    frame[idx++] = (feed >> 8) & 0xFF;
+
+    // 按键 (1 byte) - 用于能量机关模式标识
+    frame[idx++] = key;
+
+    // CRC8 (数据区: frame[2]到frame[12], 共11字节)
+    // 当前固定为0xCC
+    frame[idx++] = 0xCC;
+
+    // 帧尾
+    frame[idx++] = FRAME_TAIL;
+
+    serial.WriteData(frame, sizeof(frame));
     send_cnt += 1;
 //    LOGM(STR_CTR(WORD_LIGHT_PURPLE, "send"));
 }

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

@@ -6,7 +6,7 @@
 #define _CAMERA_WRAPPER_H_
 
 #include <additions.h>
-#include <opencv2/core/core.hpp>
+#include <opencv2/core.hpp>
 #include <camera/wrapper_head.h>
 
 #ifdef Windows
@@ -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;
@@ -33,7 +33,7 @@ private:
     tSdkCameraCapbility tCapability;
     tSdkFrameHead frame_info;
     BYTE *pby_buffer;
-    IplImage* iplImage;
+    cv::Mat image_header;
     int channel;
 
     RoundQueue<cv::Mat, 2> src_queue;
@@ -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/include/log.h

@@ -32,6 +32,7 @@
 
 #include <stdio.h>
 #include <systime.h>
+#include <iostream>
 
 /************** Define the control code *************/
 #define START_CTR           "\033[0"

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

@@ -17,7 +17,6 @@ CameraWrapper::CameraWrapper(int exposure, int gain, int camera_mode, const std:
         mode(camera_mode),
         camera_cnts(2),
         camera_status(-1),
-        iplImage(nullptr),
         rgb_buffer(nullptr),
         channel(3),
         gain(gain),
@@ -27,9 +26,11 @@ CameraWrapper::CameraWrapper(int exposure, int gain, int camera_mode, const std:
 void cameraCallback(CameraHandle hCamera, BYTE *pFrameBuffer, tSdkFrameHead* pFrameHead,PVOID pContext){
     CameraWrapper *c = (CameraWrapper*)pContext;
     CameraImageProcess(hCamera, pFrameBuffer, c->rgb_buffer, pFrameHead);
-    auto iplImage = cvCreateImageHeader(cvSize(pFrameHead->iWidth, pFrameHead->iHeight), IPL_DEPTH_8U, c->channel);
-    cvSetData(iplImage, c->rgb_buffer, pFrameHead->iWidth * c->channel);  //此处只是设置指针，无图像块数据拷贝，不需担心转换效率
-    c->src_queue.push(cv::cvarrToMat(iplImage).clone());
+    // 使用 cv::Mat 替代 IplImage
+    cv::Mat img(pFrameHead->iHeight, pFrameHead->iWidth, 
+                c->channel == 3 ? CV_8UC3 : CV_8UC1, 
+                c->rgb_buffer, pFrameHead->iWidth * c->channel);
+    c->src_queue.push(img.clone());
 }
 
 bool CameraWrapper::init() {
@@ -37,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!");
@@ -134,15 +136,9 @@ bool CameraWrapper::read(cv::Mat &src) {
 
 bool CameraWrapper::readRaw(cv::Mat &src) {
     if (CameraGetImageBuffer(h_camera, &frame_info, &pby_buffer, 500) == CAMERA_STATUS_SUCCESS) {
-        if (iplImage) {
-            cvReleaseImageHeader(&iplImage);
-        }
-
-        iplImage = cvCreateImageHeader(cvSize(frame_info.iWidth, frame_info.iHeight), IPL_DEPTH_8U, 1);
-
-        cvSetData(iplImage, pby_buffer, frame_info.iWidth);  //此处只是设置指针，无图像块数据拷贝，不需担心转换效率
-
-        src = cv::cvarrToMat(iplImage).clone();
+        // 使用 cv::Mat 替代 IplImage
+        cv::Mat img(frame_info.iHeight, frame_info.iWidth, CV_8UC1, pby_buffer, frame_info.iWidth);
+        src = img.clone();
 
         //在成功调用CameraGetImageBuffer后，必须调用CameraReleaseImageBuffer来释放获得的buffer。
         //否则再次调用CameraGetImageBuffer时，程序将被挂起一直阻塞，直到其他线程中调用CameraReleaseImageBuffer来释放了buffer
@@ -156,16 +152,13 @@ bool CameraWrapper::readRaw(cv::Mat &src) {
 }
 
 bool CameraWrapper::readProcessed(cv::Mat &src) {
-//	cerr << "Get-1" << endl;
     if (CameraGetImageBuffer(h_camera, &frame_info, &pby_buffer, 500) == CAMERA_STATUS_SUCCESS) {
         CameraImageProcess(h_camera, pby_buffer, rgb_buffer,
                            &frame_info);  // this function is super slow, better not to use it.
-        if (iplImage) {
-            cvReleaseImageHeader(&iplImage);
-        }
-        iplImage = cvCreateImageHeader(cvSize(frame_info.iWidth, frame_info.iHeight), IPL_DEPTH_8U, channel);
-        cvSetData(iplImage, rgb_buffer, frame_info.iWidth * channel);  //此处只是设置指针，无图像块数据拷贝，不需担心转换效率
-        src = cv::cvarrToMat(iplImage).clone();
+        // 使用 cv::Mat 替代 IplImage
+        int mat_type = (channel == 3) ? CV_8UC3 : CV_8UC1;
+        cv::Mat img(frame_info.iHeight, frame_info.iWidth, mat_type, rgb_buffer, frame_info.iWidth * channel);
+        src = img.clone();
         //在成功调用CameraGetImageBuffer后，必须调用CameraReleaseImageBuffer来释放获得的buffer。
         //否则再次调用CameraGetImageBuffer时，程序将被挂起一直阻塞，直到其他线程中调用CameraReleaseImageBuffer来释放了buffer
         CameraReleaseImageBuffer(h_camera, pby_buffer);

others/src/options.cpp

@@ -6,19 +6,20 @@
 #include <log.h>
 #include <cstring>
 #include <map>
+#include <string>
 
-bool show_armor_box = false;
+bool show_armor_box = true;
 bool show_armor_boxes = false;
 bool show_light_blobs = false;
 bool show_origin = false;
-bool run_with_camera = false;
+bool run_with_camera = true;
 bool save_video = false;
 bool wait_uart = false;
 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;

tools/tty-permission.sh

@@ -0,0 +1,3 @@
+#!/bin/sh
+
+sudo touch /etc/udev/rules.d/70-ttyusb.rules