From e62c53d343628d8397a3614c1ca874b1c8eb20b3 Mon Sep 17 00:00:00 2001 From: sun Date: Tue, 23 Jul 2019 16:02:50 +0800 Subject: [PATCH] energy change --- CMakeLists.txt | 2 +- energy/include/energy/energy.h | 5 +- energy/src/energy/change/multiple_mode.cpp | 26 ++++++ energy/src/energy/clear/energy_init.cpp | 11 +-- energy/src/energy/find/energy_finder.cpp | 60 +++++++++++--- energy/src/energy/find/target_finder.cpp | 2 +- energy/src/energy/get/aim_point_get.cpp | 12 +-- energy/src/energy/get/origin_get.cpp | 15 ++-- energy/src/energy/judge/judge_shoot.cpp | 15 ++-- energy/src/energy/run.cpp | 38 ++------- energy/src/energy/send/send.cpp | 45 ++++++++-- main.cpp | 96 ++++++++++++---------- others/include/additions/additions.h | 2 + others/src/additions/additions.cpp | 1 + 14 files changed, 204 insertions(+), 126 deletions(-) create mode 100644 energy/src/energy/change/multiple_mode.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index e19eb1d..8325866 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -2,7 +2,7 @@ PROJECT(SJTU-RM-CV) SET(CMAKE_CXX_STANDARD 11) -SET(CMAKE_BUILD_TYPE RELEASE) +SET(CMAKE_BUILD_TYPE DEBUG) SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O3") SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DPATH=\"\\\"${PROJECT_SOURCE_DIR}\\\"\"") SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -D${CMAKE_SYSTEM_NAME}") diff --git a/energy/include/energy/energy.h b/energy/include/energy/energy.h index 5a3c6ec..27bf522 100644 --- a/energy/include/energy/energy.h +++ b/energy/include/energy/energy.h @@ -35,6 +35,7 @@ public: void setEnergyInit();//设置能量机关初始化 void setBigEnergyInit();//设置大能量机关初始化 void setSmallEnergyInit();//设置小能量机关初始化 + void sendEnergy();//发送能量机关数据 void sendTarget(Serial& serial, float x, float y, float z);//发送数据 void sendTarget(Serial& serial, float x, float y, float z, uint16_t u);//发送数据 @@ -64,6 +65,7 @@ private: double radius;//大风车半径 int send_cnt;//向主控板发送的数据总次数 + int camera_cnt;//摄像头数量 int last_fans_cnt;//上一帧的扇叶个数 int guess_devide;//刚进入猜测状态时，猜测目标点在极坐标中的分区 int energy_rotation_direction;//风车旋转方向 @@ -153,8 +155,9 @@ private: void writeDownMark();//记录操作手标定的云台初始角度 bool guessTarget();//获得猜测击打点位 - void changeTarget();//判断目标是否改变 bool getOrigin();//获得云台对心所需角度 + void changeTarget();//判断目标是否改变 + void multipleMode(cv::Mat gimbal_src);//多模式切换 void getTargetPolarAngle();//获得目标装甲板极坐标角度 void getPredictPoint(cv::Point target_point);//获取预测点位 void getAimPoint(cv::Point target_point);//通过自瞄逻辑计算点位 diff --git a/energy/src/energy/change/multiple_mode.cpp b/energy/src/energy/change/multiple_mode.cpp new file mode 100644 index 0000000..3fdfe2c --- /dev/null +++ b/energy/src/energy/change/multiple_mode.cpp @@ -0,0 +1,26 @@ +// +// Created by sun on 19-7-23. +// + +#include "energy/energy.h" + +using namespace std; +using namespace cv; + +void Energy::multipleMode(cv::Mat gimbal_src) { + if (is_predicting) { + getPredictPoint(target_point); + getAimPoint(predict_point); + judgeShootInGimbal(); + sendEnergy(); + } else if (is_guessing && stayGuessing()) { + findFans(gimbal_src); + if (show_energy)showFans("fans", gimbal_src); + if (save_mark)writeDownMark(); + guessTarget(); + if (show_energy)showGuessTarget("guess", gimbal_src); + getPredictPoint(guess_point); + getAimPoint(predict_point); + sendEnergy(); + } +} \ No newline at end of file diff --git a/energy/src/energy/clear/energy_init.cpp b/energy/src/energy/clear/energy_init.cpp index 0d1f34d..1db2106 100644 --- a/energy/src/energy/clear/energy_init.cpp +++ b/energy/src/energy/clear/energy_init.cpp @@ -29,6 +29,7 @@ void Energy::initEnergy() { radius = 0; send_cnt = 0; + camera_cnt = 1; last_fans_cnt = 0; guess_devide = 0; energy_rotation_direction = ANTICLOCKWISE; @@ -101,13 +102,13 @@ void Energy::initEnergyPartParam() { gimbal_energy_part_param_.CENTER_R_CONTOUR_AREA_MAX = 100000; gimbal_energy_part_param_.CENTER_R_CONTOUR_AREA_MIN = 0; - gimbal_energy_part_param_.CENTER_R_CONTOUR_LENGTH_MIN = 7; - gimbal_energy_part_param_.CENTER_R_CONTOUR_LENGTH_MAX = 25; - gimbal_energy_part_param_.CENTER_R_CONTOUR_WIDTH_MIN = 7; - gimbal_energy_part_param_.CENTER_R_CONTOUR_WIDTH_MAX = 25; + gimbal_energy_part_param_.CENTER_R_CONTOUR_LENGTH_MIN = 5; + gimbal_energy_part_param_.CENTER_R_CONTOUR_LENGTH_MAX = 45; + gimbal_energy_part_param_.CENTER_R_CONTOUR_WIDTH_MIN = 5; + gimbal_energy_part_param_.CENTER_R_CONTOUR_WIDTH_MAX = 45; gimbal_energy_part_param_.CENTER_R_CONTOUR_HW_RATIO_MAX = 3; gimbal_energy_part_param_.CENTER_R_CONTOUR_HW_RATIO_MIN = 1; - gimbal_energy_part_param_.CENTER_R_CONTOUR_AREA_RATIO_MIN = 0.5; + gimbal_energy_part_param_.CENTER_R_CONTOUR_AREA_RATIO_MIN = 0.3; gimbal_energy_part_param_.CENTER_R_CONTOUR_INTERSETION_AREA_MIN = 10; gimbal_energy_part_param_.FLOW_STRIP_FAN_CONTOUR_AREA_MAX = 3000; diff --git a/energy/src/energy/find/energy_finder.cpp b/energy/src/energy/find/energy_finder.cpp index 8767ff4..8fb3400 100644 --- a/energy/src/energy/find/energy_finder.cpp +++ b/energy/src/energy/find/energy_finder.cpp @@ -21,7 +21,7 @@ int Energy::findFans(const cv::Mat src) { } std::vector > fan_contours; FanStruct(src_bin);//图像膨胀，防止图像断开并更方便寻找 -// imshow("fan struct",src_bin); + if (show_process)imshow("fan struct", src_bin); findContours(src_bin, fan_contours, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_NONE); for (auto &fan_contour : fan_contours) { @@ -69,7 +69,7 @@ int Energy::findArmors(const cv::Mat src) { ArmorStruct(src_bin);//图像膨胀，防止图像断开并更方便寻找 findContours(src_bin, armor_contours, CV_RETR_LIST, CV_CHAIN_APPROX_NONE); - imshow("armor struct", src_bin); + if (show_process)imshow("armor struct", src_bin); // findContours(src_bin, armor_contours_external, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_NONE); // for (int i = 0; i < armor_contours_external.size(); i++)//去除外轮廓 @@ -121,7 +121,7 @@ bool Energy::findCenterR(const cv::Mat src) { } std::vector > center_R_contours; CenterRStruct(src_bin); -// imshow("R struct",src_bin); + if (show_process)imshow("R struct", src_bin); findContours(src_bin, center_R_contours, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_NONE); for (auto ¢er_R_contour : center_R_contours) { if (!isValidCenterRContour(center_R_contour)) { @@ -172,15 +172,16 @@ bool Energy::findFlowStripFan(const cv::Mat src) { } std::vector > flow_strip_fan_contours; FlowStripFanStruct(src_bin);//图像膨胀，防止图像断开并更方便寻找 - imshow("flow strip fan struct", 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); + std::vector candidate_flow_strip_fans; for (auto &flow_strip_fan_contour : flow_strip_fan_contours) { if (!isValidFlowStripFanContour(src_bin, flow_strip_fan_contour)) { continue; } - flow_strip_fan = cv::minAreaRect(flow_strip_fan_contour); + candidate_flow_strip_fans.emplace_back(cv::minAreaRect(flow_strip_fan_contour)); // RotatedRect cur_rect = minAreaRect(flow_strip_fan_contour); // Size2f cur_size = cur_rect.size; @@ -196,12 +197,51 @@ bool Energy::findFlowStripFan(const cv::Mat src) { // cout<= 2) { + //用锤子筛选仍然有多个候选区，进一步用锤头做筛选 + std::vector candidate_target_fans; + for (auto candidate_flow_strip_fan: candidate_flow_strip_fans) { + flow_strip_fan = candidate_flow_strip_fan; + if (!findTargetInFlowStripFan()) { + continue; + } + candidate_target_fans.emplace_back(candidate_flow_strip_fan); + } + if (candidate_target_fans.size() == 1) { + flow_strip_fan = candidate_target_fans.at(0); + return true; + } else if(candidate_target_fans.empty()){ + cout<<"No candidate target fan contains a target armor!"< mask_rect; + target_armor.points(vertices); + for (int i = 0; i < 4; i++) + line(src_mask, vertices[i], vertices[(i + 1) % 4], Scalar(0, 0, 0), 20); + if (findFlowStrip(src_mask)) { + flow_strip_fan = candidate_target_fan; + return true; + } + } + cout<<"No candidate target fan contains a flow strip!"< > flow_strip_contours; findContours(src_bin, flow_strip_contours, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_NONE); diff --git a/energy/src/energy/find/target_finder.cpp b/energy/src/energy/find/target_finder.cpp index 4afa9ba..4e5a82c 100644 --- a/energy/src/energy/find/target_finder.cpp +++ b/energy/src/energy/find/target_finder.cpp @@ -68,7 +68,7 @@ bool Energy::findTargetInFlowStripFan() { return true; } } -// cout<<"find target false"<(atan(dx / FOCUS_PIXAL) * 180 / PI); center_delta_pitch = static_cast(atan(dy / FOCUS_PIXAL) * 180 / PI); + shoot = 1; + sendEnergy(); if (abs(center_delta_yaw) > 0.3 || abs(center_delta_pitch) > 0.3) { -// cout << "origin not get!" << endl; -// cout << center_delta_yaw << '\t' << center_delta_pitch << endl; - sendTarget(serial, center_delta_yaw, center_delta_pitch, 0); return false; } else { origin_yaw = mcuData.curr_yaw; origin_pitch = mcuData.curr_pitch; auto_mark = true; - sendTarget(serial, center_delta_yaw, center_delta_pitch, 1); LOGM(STR_CTR(WORD_BLUE_CODE, "auto mark success!")); return true; } diff --git a/energy/src/energy/judge/judge_shoot.cpp b/energy/src/energy/judge/judge_shoot.cpp index 5edd7c2..10c765a 100644 --- a/energy/src/energy/judge/judge_shoot.cpp +++ b/energy/src/energy/judge/judge_shoot.cpp @@ -13,8 +13,8 @@ using namespace cv; //---------------------------------------------------------------------------------------------------------------------- // 此函数用于判断世界坐标系下是否可以发弹 // --------------------------------------------------------------------------------------------------------------------- -void Energy::judgeShootInWorld(){ - if (abs(yaw_rotation - mcuData.curr_yaw) < 0.8 && fabs(pitch_rotation - mcuData.curr_pitch) < 0.8) { +void Energy::judgeShootInWorld() { + if (abs(yaw_rotation - mcuData.curr_yaw) < 0.5 && abs(pitch_rotation - mcuData.curr_pitch) < 0.5) { shoot = 4; is_predicting = false; is_guessing = true; @@ -29,19 +29,16 @@ void Energy::judgeShootInWorld(){ //---------------------------------------------------------------------------------------------------------------------- // 此函数用于判断云台坐标系下是否可以发弹 // --------------------------------------------------------------------------------------------------------------------- -void Energy::judgeShootInGimbal(){ +void Energy::judgeShootInGimbal() { if (abs(yaw_rotation) < 0.5 && fabs(pitch_rotation) < 0.5) { - shoot = 4; + shoot = 3; is_predicting = false; is_guessing = true; start_guess = true; gettimeofday(&time_start_guess, NULL); // LOGM(STR_CTR(WORD_LIGHT_RED, "Start Guessing!")); } else - shoot = 2; + shoot = 1; - sum_yaw += yaw_rotation; - sum_pitch += pitch_rotation; - yaw_rotation = AIM_KP * yaw_rotation + AIM_KI * sum_yaw; - pitch_rotation = AIM_KP * pitch_rotation + AIM_KI * sum_pitch; +// cout << "yaw: " << yaw_rotation << '\t' << "pitch: " << pitch_rotation << endl; } diff --git a/energy/src/energy/run.cpp b/energy/src/energy/run.cpp index 10a5a2d..7f6e8c4 100644 --- a/energy/src/energy/run.cpp +++ b/energy/src/energy/run.cpp @@ -19,8 +19,7 @@ void Energy::runBig(cv::Mat &gimbal_src, cv::Mat &chassis_src) { energy_part_param_ = gimbal_energy_part_param_; clearAll(); initImage(gimbal_src); -// findFans(gimbal_src); -// showFans("fan",gimbal_src); + camera_cnt = 2; if (findArmors(gimbal_src) < 1)return; if (show_energy)showArmors("armor", gimbal_src); @@ -46,7 +45,6 @@ void Energy::runBig(cv::Mat &gimbal_src, cv::Mat &chassis_src) { if (findArmors(chassis_src) < 1)return; if (show_energy)showArmors("armor", chassis_src); if (!findFlowStripFan(chassis_src))return; - showFlowStripFan("flow strip fan", chassis_src); if (!findTargetInFlowStripFan()) return; if (!findCenterROI(chassis_src))return; if (show_energy)showFlowStripFan("strip", chassis_src); @@ -61,7 +59,7 @@ void Energy::runBig(cv::Mat &gimbal_src, cv::Mat &chassis_src) { getPredictPoint(target_point); gimbalRotation(); judgeShootInWorld(); - sendTarget(serial, yaw_rotation, pitch_rotation, change_target); + sendEnergy(); } } @@ -74,9 +72,6 @@ void Energy::runBig(cv::Mat &gimbal_src) { clearAll(); initImage(gimbal_src); - changeMark(); - if (is_mark)return;//操作手强制手动标定origin_yaw和origin_pitch - if (show_process)imshow("bin", gimbal_src); if (findArmors(gimbal_src) < 1)return; if (show_energy)showArmors("armor", gimbal_src); @@ -88,7 +83,6 @@ void Energy::runBig(cv::Mat &gimbal_src) { if (show_energy)showCenterR("R", gimbal_src); changeTarget(); getTargetPolarAngle(); -// if (!getOrigin())return; if (energy_rotation_init) { initRotation(); return; @@ -96,7 +90,7 @@ void Energy::runBig(cv::Mat &gimbal_src) { getPredictPoint(target_point); getAimPoint(predict_point); judgeShootInGimbal(); - sendTarget(serial, yaw_rotation, pitch_rotation, change_target); + sendEnergy(); } @@ -114,31 +108,11 @@ void Energy::runSmall(cv::Mat &gimbal_src) { if (!findFlowStripFan(gimbal_src))return; if (!findTargetInFlowStripFan()) return; -// if (!findCenterROI(gimbal_src))return; -// if (show_energy)showFlowStripFan("strip", gimbal_src); -// if (!findCenterR(gimbal_src))return; -// if (show_energy)showCenterR("R", gimbal_src); - changeTarget(); - getAimPoint(target_point); + getPredictPoint(target_point); + getAimPoint(predict_point); judgeShootInGimbal(); - sendTarget(serial, yaw_rotation, pitch_rotation, change_target); + sendEnergy(); } -//if (is_predicting) { -//getPredictPoint(target_point); -//getAimPoint(predict_point); -//cout << yaw_rotation << '\t' << pitch_rotation << endl; -//judgeShootInGimbal(); -//sendTarget(serial, yaw_rotation, pitch_rotation, shoot); -//} else if (is_guessing && stayGuessing()) { -//findFans(gimbal_src); -//if (show_energy)showFans("fans", gimbal_src); -//if (save_mark)writeDownMark(); -//guessTarget(); -//if (show_energy)showGuessTarget("guess", gimbal_src); -//getPredictPoint(guess_point); -//getAimPoint(predict_point); -//sendTarget(serial, yaw_rotation, pitch_rotation, 5); -//} \ No newline at end of file diff --git a/energy/src/energy/send/send.cpp b/energy/src/energy/send/send.cpp index f50011a..c395477 100644 --- a/energy/src/energy/send/send.cpp +++ b/energy/src/energy/send/send.cpp @@ -4,15 +4,46 @@ #include "energy/energy.h" #include #include "log.h" +#include "config/setconfig.h" using namespace std; + +//---------------------------------------------------------------------------------------------------------------------- +// 此函数用于发送能量机关数据 +// --------------------------------------------------------------------------------------------------------------------- +void Energy::sendEnergy() { + if (is_big) { + if (camera_cnt == 1) { + sum_yaw += yaw_rotation; + sum_pitch += pitch_rotation; + yaw_rotation = AIM_KP * yaw_rotation + AIM_KI * sum_yaw; + pitch_rotation = AIM_KP * pitch_rotation + AIM_KI * sum_pitch; + } else if (is_chassis){ + sum_yaw += yaw_rotation - mcuData.curr_yaw; + sum_pitch += pitch_rotation - mcuData.curr_pitch; + yaw_rotation = AIM_KP * yaw_rotation + AIM_KI * sum_yaw; + pitch_rotation = AIM_KP * pitch_rotation + AIM_KI * sum_pitch; + } + } + + if (change_target) { + sendTarget(serial, yaw_rotation, pitch_rotation, 5); + } else if (is_predicting) { + sendTarget(serial, yaw_rotation, pitch_rotation, shoot); + } else { + sendTarget(serial, yaw_rotation, pitch_rotation, 6); + } + +} + + //---------------------------------------------------------------------------------------------------------------------- // 此函数用于发送数据给主控板 // --------------------------------------------------------------------------------------------------------------------- -void Energy::sendTarget(Serial& serial, float x, float y, float z){ +void Energy::sendTarget(Serial &serial, float x, float y, float z) { short x_tmp, y_tmp, z_tmp; - uint8_t buff[10]; + uint8_t buff[8]; #ifdef WITH_COUNT_FPS static auto last_time = time(nullptr); @@ -36,19 +67,17 @@ void Energy::sendTarget(Serial& serial, float x, float y, float z){ buff[4] = static_cast((y_tmp >> 0) & 0xFF); buff[5] = static_cast((z_tmp >> 8) & 0xFF); buff[6] = static_cast((z_tmp >> 0) & 0xFF); - buff[9] = 'e'; + buff[7] = 'e'; serial.WriteData(buff, sizeof(buff)); - send_cnt+=1; + send_cnt += 1; // LOGM(STR_CTR(WORD_LIGHT_PURPLE, "send")); } - - //---------------------------------------------------------------------------------------------------------------------- // 此函数用于发送数据给主控板 // --------------------------------------------------------------------------------------------------------------------- -void Energy::sendTarget(Serial& serial, float x, float y, float z, uint16_t u){ +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]; @@ -78,6 +107,6 @@ void Energy::sendTarget(Serial& serial, float x, float y, float z, uint16_t u){ buff[8] = static_cast((u >> 0) & 0xFF);; buff[9] = 'e'; serial.WriteData(buff, sizeof(buff)); - send_cnt+=1; + send_cnt += 1; // LOGM(STR_CTR(WORD_LIGHT_PURPLE, "send")); } diff --git a/main.cpp b/main.cpp index fe1b3f4..6da354d 100644 --- a/main.cpp +++ b/main.cpp @@ -34,6 +34,8 @@ mcu_data mcuData = { // 单片机端回传结构体 0, // 云台角度标记位 1, // 是否启用数字识别 ENEMY_RED, // 敌方颜色 + 0, // 能量机关x轴补偿量 + 0, // 能量机关y轴补偿量 }; WrapperHead *video_gimbal = nullptr; // 云台摄像头视频源 @@ -64,8 +66,8 @@ int main(int argc, char *argv[]) { video_gimbal = new CameraWrapper(ARMOR_CAMERA_GAIN, 0/*, "armor"*/); video_chassis = new CameraWrapper(ENERGY_CAMERA_GAIN, 0/*, "energy"*/); } else { - video_gimbal = new VideoWrapper("/home/sun/项目/energy_video/gimbal132.avi"); - video_chassis = new VideoWrapper("/home/sun/项目/energy_video/gimbal132.avi"); + video_gimbal = new VideoWrapper("/home/sun/项目/energy_video/gimbal255.avi"); + video_chassis = new VideoWrapper("/home/sun/项目/energy_video/gimbal255.avi"); } if (video_gimbal->init()) { LOGM("video_gimbal source initialization successfully."); @@ -92,9 +94,10 @@ int main(int argc, char *argv[]) { cout << "start running" << endl; do { // CNT_TIME("Total", { - if (mcuData.state == BIG_ENERGY_STATE) {//大能量机关模式 - if (last_state != BIG_ENERGY_STATE) {//若上一帧不是大能量机关模式，即刚往完成切换，则需要初始化 - destroyAllWindows(); + if (mcuData.state == BIG_ENERGY_STATE) {//大能量机关模式 + if (last_state != BIG_ENERGY_STATE) {//若上一帧不是大能量机关模式，即刚往完成切换，则需要初始化 + destroyAllWindows(); + if(from_camera){ delete video_gimbal; video_gimbal = new CameraWrapper(ENERGY_CAMERA_GAIN, 0/*, "armor"*/); if (video_gimbal->init()) { @@ -102,26 +105,28 @@ int main(int argc, char *argv[]) { } else { LOGW("video_gimbal source unavailable!"); } - energy.setBigEnergyInit(); - checkReconnect(video_chassis->read(chassis_src)); + } + checkReconnect(video_chassis->read(chassis_src)); + energy.setBigEnergyInit(); + } + ok = checkReconnect(video_gimbal->read(gimbal_src)); + video_chassis->read(chassis_src); +#ifdef GIMBAL_FLIP_MODE + flip(gimbal_src, gimbal_src, GIMBAL_FLIP_MODE); +#endif #ifdef CHASSIS_FLIP_MODE - flip(chassis_src, chassis_src, CHASSIS_FLIP_MODE); + flip(chassis_src, chassis_src, CHASSIS_FLIP_MODE); #endif - } - ok = checkReconnect(video_gimbal->read(gimbal_src)); - video_chassis->read(chassis_src); -#ifdef GIMBAL_FLIP_MODE - flip(gimbal_src, gimbal_src, GIMBAL_FLIP_MODE); -#endif - if (!from_camera) extract(gimbal_src, chassis_src); - if (save_video) saveVideos(gimbal_src, chassis_src);//保存视频 - if (show_origin) showOrigin(gimbal_src, chassis_src);//显示原始图像 - energy.runBig(gimbal_src, chassis_src); + if (!from_camera) extract(gimbal_src, chassis_src); + if (save_video) saveVideos(gimbal_src, chassis_src);//保存视频 + if (show_origin) showOrigin(gimbal_src, chassis_src);//显示原始图像 + energy.runBig(gimbal_src, chassis_src); // energy.runBig(gimbal_src); - last_state = mcuData.state;//更新上一帧状态 - } else if (mcuData.state == SMALL_ENERGY_STATE) { - if (mcuData.state != SMALL_ENERGY_STATE) { - destroyAllWindows(); + last_state = mcuData.state;//更新上一帧状态 + } else if (mcuData.state == SMALL_ENERGY_STATE) { + if (mcuData.state != SMALL_ENERGY_STATE) { + destroyAllWindows(); + if(from_camera){ delete video_gimbal; video_gimbal = new CameraWrapper(ENERGY_CAMERA_GAIN, 0/*, "armor"*/); if (video_gimbal->init()) { @@ -129,20 +134,22 @@ int main(int argc, char *argv[]) { } else { LOGW("video_gimbal source unavailable!"); } - energy.setSmallEnergyInit(); } - ok = checkReconnect(video_gimbal->read(gimbal_src)); + energy.setSmallEnergyInit(); + } + ok = checkReconnect(video_gimbal->read(gimbal_src)); #ifdef GIMBAL_FLIP_MODE - flip(gimbal_src, gimbal_src, GIMBAL_FLIP_MODE); + flip(gimbal_src, gimbal_src, GIMBAL_FLIP_MODE); #endif - if (!from_camera) extract(gimbal_src); - if (save_video) saveVideos(gimbal_src);//保存视频 - if (show_origin) showOrigin(gimbal_src);//显示原始图像 - energy.runSmall(gimbal_src); - last_state = mcuData.state;//更新上一帧状态 - } else { // 自瞄模式 - if (last_state != ARMOR_STATE) { - destroyAllWindows(); + if (!from_camera) extract(gimbal_src); + if (save_video) saveVideos(gimbal_src);//保存视频 + if (show_origin) showOrigin(gimbal_src);//显示原始图像 + energy.runSmall(gimbal_src); + last_state = mcuData.state;//更新上一帧状态 + } else { // 自瞄模式 + if (last_state != ARMOR_STATE) { + destroyAllWindows(); + if(from_camera){ delete video_gimbal; video_gimbal = new CameraWrapper(ARMOR_CAMERA_GAIN, 0/*, "armor"*/); if (video_gimbal->init()) { @@ -151,18 +158,19 @@ int main(int argc, char *argv[]) { LOGW("video_gimbal source unavailable!"); } } - last_state = mcuData.state; - ok = checkReconnect(video_gimbal->read(gimbal_src)); -#ifdef GIMBAL_FLIP_MODE - flip(gimbal_src, gimbal_src, GIMBAL_FLIP_MODE); -#endif - if (!from_camera) extract(gimbal_src); -// if (save_video) saveVideos(gimbal_src); - if (show_origin) showOrigin(gimbal_src); - CNT_TIME("Armor Time", { - armorFinder.run(gimbal_src); - }); } + last_state = mcuData.state; + ok = checkReconnect(video_gimbal->read(gimbal_src)); +#ifdef GIMBAL_FLIP_MODE + flip(gimbal_src, gimbal_src, GIMBAL_FLIP_MODE); +#endif + if (!from_camera) extract(gimbal_src); +// if (save_video) saveVideos(gimbal_src); + if (show_origin) showOrigin(gimbal_src); + CNT_TIME("Armor Time", { + armorFinder.run(gimbal_src); + }); + } // cv::waitKey(0); // }); } while (ok); diff --git a/others/include/additions/additions.h b/others/include/additions/additions.h index 254e13b..be00e7e 100644 --- a/others/include/additions/additions.h +++ b/others/include/additions/additions.h @@ -15,6 +15,8 @@ struct mcu_data{ uint8_t mark; uint8_t use_classifier; uint8_t enemy_color; + int delta_x; + int delta_y; }; extern mcu_data mcuData; diff --git a/others/src/additions/additions.cpp b/others/src/additions/additions.cpp index e8373ec..29946b2 100644 --- a/others/src/additions/additions.cpp +++ b/others/src/additions/additions.cpp @@ -39,6 +39,7 @@ void uartReceive(Serial *pSerial) { memcpy(&mcuData, buffer, sizeof(mcuData)); // LOGM("Get, state:%c, mark:%d!", mcuData.state, (int) mcuData.mark); // LOGM("Get yaw: %f, pitch: %f!", mcuData.curr_yaw, mcuData.curr_pitch); +// LOGM("Get delta x: %d, delta y: %d!", mcuData.delta_x, mcuData.delta_y); // static int t = time(nullptr); // static int cnt = 0; // if(time(nullptr) > t){