From e1fc19fdbf9a313a36818cb94d85df91e49d072a Mon Sep 17 00:00:00 2001 From: sun Date: Mon, 5 Aug 2019 17:27:49 +0800 Subject: [PATCH 1/6] energy changed --- energy/src/energy/clear/energy_init.cpp | 3 +- energy/src/energy/run.cpp | 2 + energy/src/energy/send/send.cpp | 1 + main.cpp | 144 ++++++++++++------------ 4 files changed, 77 insertions(+), 73 deletions(-) diff --git a/energy/src/energy/clear/energy_init.cpp b/energy/src/energy/clear/energy_init.cpp index 80caba9..afd640d 100644 --- a/energy/src/energy/clear/energy_init.cpp +++ b/energy/src/energy/clear/energy_init.cpp @@ -82,7 +82,8 @@ void Energy::initEnergy() { void Energy::initEnergyPartParam() { // gimbal_energy_part_param_.GRAY_THRESH = 120;//home // gimbal_energy_part_param_.GRAY_THRESH = 200;//official - gimbal_energy_part_param_.GRAY_THRESH = 180;//game +// gimbal_energy_part_param_.GRAY_THRESH =180;//single game + gimbal_energy_part_param_.GRAY_THRESH =100;//game gimbal_energy_part_param_.SPLIT_GRAY_THRESH = 180; gimbal_energy_part_param_.FAN_GRAY_THRESH = 75; gimbal_energy_part_param_.ARMOR_GRAY_THRESH = 80; diff --git a/energy/src/energy/run.cpp b/energy/src/energy/run.cpp index 9cf3006..610ed61 100644 --- a/energy/src/energy/run.cpp +++ b/energy/src/energy/run.cpp @@ -107,6 +107,8 @@ void Energy::runBig(cv::Mat &gimbal_src) { getPredictPoint(target_point); getAimPoint(predict_point); judgeShootInGimbal(); +// cout << "yaw: " << yaw_rotation << '\t' << "pitch: " << pitch_rotation << '\t' << "shoot: " << shoot << endl; +// waitKey(0); sendEnergy(); } diff --git a/energy/src/energy/send/send.cpp b/energy/src/energy/send/send.cpp index 2feaa77..f310129 100644 --- a/energy/src/energy/send/send.cpp +++ b/energy/src/energy/send/send.cpp @@ -25,6 +25,7 @@ void Energy::sendEnergy() { yaw_rotation = BIG_YAW_AIM_KP * yaw_rotation + BIG_YAW_AIM_KI * sum_yaw + BIG_YAW_AIM_KD * (yaw_rotation - last_yaw); pitch_rotation = BIG_PITCH_AIM_KP * pitch_rotation + BIG_PITCH_AIM_KI * sum_pitch + BIG_PITCH_AIM_KD * (pitch_rotation - last_pitch); + MINMAX(yaw_rotation, -10, 10); last_yaw = tmp_yaw; last_pitch = tmp_pitch; } else if (is_chassis) { diff --git a/main.cpp b/main.cpp index 10671d7..850a291 100644 --- a/main.cpp +++ b/main.cpp @@ -30,7 +30,7 @@ using namespace std; McuData mcu_data = { // 单片机端回传结构体 0, // 当前云台yaw角 0, // 当前云台pitch角 - ARMOR_STATE, // 当前状态，自瞄-大符-小符 + BIG_ENERGY_STATE, // 当前状态，自瞄-大符-小符 0, // 云台角度标记位 1, // 是否启用数字识别 ENEMY_RED, // 敌方颜色 @@ -66,8 +66,8 @@ int main(int argc, char *argv[]) { video_gimbal = new CameraWrapper(ARMOR_CAMERA_GAIN, 2/*, "armor"*/); video_chassis = new CameraWrapper(ENERGY_CAMERA_GAIN, 2/*, "energy"*/); } else { - video_gimbal = new VideoWrapper(PROJECT_DIR"/gimbal_video/0.avi"); - video_chassis = new VideoWrapper(PROJECT_DIR"/gimbal_video/0.avi"); + video_gimbal = new VideoWrapper("/home/sun/桌面/shiying/blue_1.avi"); + video_chassis = new VideoWrapper("/home/sun/桌面/shiying/blue_1.avi"); } if (video_gimbal->init()) { LOGM("video_gimbal source initialization successfully."); @@ -94,90 +94,90 @@ int main(int argc, char *argv[]) { cout << "start running" << endl; do { CNT_TIME("Total", { - if (mcu_data.state == BIG_ENERGY_STATE) {//大能量机关模式 - if (last_state != BIG_ENERGY_STATE) {//若上一帧不是大能量机关模式，即刚往完成切换，则需要初始化 - LOGM(STR_CTR(WORD_BLUE, "Start Big Energy!")); - destroyAllWindows(); - if (from_camera) { - delete video_gimbal; - video_gimbal = new CameraWrapper(ENERGY_CAMERA_GAIN, 2/*, "armor"*/); - if (video_gimbal->init()) { - LOGM("video_gimbal source initialization successfully."); - } else { - LOGW("video_gimbal source unavailable!"); + if (mcu_data.state == BIG_ENERGY_STATE) {//大能量机关模式 + if (last_state != BIG_ENERGY_STATE) {//若上一帧不是大能量机关模式，即刚往完成切换，则需要初始化 + LOGM(STR_CTR(WORD_BLUE, "Start Big Energy!")); + destroyAllWindows(); + if (from_camera) { + delete video_gimbal; + video_gimbal = new CameraWrapper(ENERGY_CAMERA_GAIN, 2/*, "armor"*/); + if (video_gimbal->init()) { + LOGM("video_gimbal source initialization successfully."); + } else { + LOGW("video_gimbal source unavailable!"); + } } + checkReconnect(video_chassis->read(chassis_src)); + energy.setBigEnergyInit(); } - checkReconnect(video_chassis->read(chassis_src)); - energy.setBigEnergyInit(); - } - last_state = mcu_data.state;//更新上一帧状态 - ok = checkReconnect(video_gimbal->read(gimbal_src)); - video_chassis->read(chassis_src); + last_state = mcu_data.state;//更新上一帧状态 + ok = checkReconnect(video_gimbal->read(gimbal_src)); + video_chassis->read(chassis_src); #ifdef GIMBAL_FLIP_MODE - flip(gimbal_src, gimbal_src, 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 - 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); - } else if (mcu_data.state == SMALL_ENERGY_STATE) { - if (last_state != SMALL_ENERGY_STATE) { - LOGM(STR_CTR(WORD_GREEN, "Start Small Energy!")); - destroyAllWindows(); - if (from_camera) { - delete video_gimbal; - video_gimbal = new CameraWrapper(ENERGY_CAMERA_GAIN, 2/*, "armor"*/); - if (video_gimbal->init()) { - LOGM("video_gimbal source initialization successfully."); - } else { - LOGW("video_gimbal source unavailable!"); + 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); + } else if (mcu_data.state == SMALL_ENERGY_STATE) { + if (last_state != SMALL_ENERGY_STATE) { + LOGM(STR_CTR(WORD_GREEN, "Start Small Energy!")); + destroyAllWindows(); + if (from_camera) { + delete video_gimbal; + video_gimbal = new CameraWrapper(ENERGY_CAMERA_GAIN, 2/*, "armor"*/); + if (video_gimbal->init()) { + LOGM("video_gimbal source initialization successfully."); + } else { + LOGW("video_gimbal source unavailable!"); + } + } + energy.setSmallEnergyInit(); + } + last_state = mcu_data.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);//显示原始图像 + energy.runSmall(gimbal_src); + } else { // 自瞄模式 + if (last_state != ARMOR_STATE) { + LOGM(STR_CTR(WORD_RED, "Start Armor!")); + destroyAllWindows(); + if (from_camera) { + delete video_gimbal; + video_gimbal = new CameraWrapper(ARMOR_CAMERA_GAIN, 2/*, "armor"*/); + if (video_gimbal->init()) { + LOGM("video_gimbal source initialization successfully."); + } else { + LOGW("video_gimbal source unavailable!"); + } } } - energy.setSmallEnergyInit(); - } - last_state = mcu_data.state;//更新上一帧状态 - ok = checkReconnect(video_gimbal->read(gimbal_src)); + last_state = mcu_data.state; + CNT_TIME(STR_CTR(WORD_GREEN, "read img"), { + if (!checkReconnect(video_gimbal->read(gimbal_src))) continue; + }); #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);//显示原始图像 - energy.runSmall(gimbal_src); - } else { // 自瞄模式 - if (last_state != ARMOR_STATE) { - LOGM(STR_CTR(WORD_RED, "Start Armor!")); - destroyAllWindows(); - if (from_camera) { - delete video_gimbal; - video_gimbal = new CameraWrapper(ARMOR_CAMERA_GAIN, 2/*, "armor"*/); - if (video_gimbal->init()) { - LOGM("video_gimbal source initialization successfully."); - } else { - LOGW("video_gimbal source unavailable!"); - } - } - } - last_state = mcu_data.state; - CNT_TIME(STR_CTR(WORD_GREEN, "read img"), { - if(!checkReconnect(video_gimbal->read(gimbal_src))) continue; - }); -#ifdef GIMBAL_FLIP_MODE - flip(gimbal_src, gimbal_src, GIMBAL_FLIP_MODE); + flip(gimbal_src, gimbal_src, GIMBAL_FLIP_MODE); #endif // CNT_TIME("something whatever", { if (!from_camera) extract(gimbal_src); if (save_video) saveVideos(gimbal_src); if (show_origin) showOrigin(gimbal_src); // }); - CNT_TIME(STR_CTR(WORD_CYAN, "Armor Time"), { - armor_finder.run(gimbal_src); - }); - } + CNT_TIME(STR_CTR(WORD_CYAN, "Armor Time"), { + armor_finder.run(gimbal_src); + }); + } // cv::waitKey(0); }); } while (ok); From b27bb48ca5b2e878b895d7b39871a120d236a9bd Mon Sep 17 00:00:00 2001 From: sun Date: Mon, 5 Aug 2019 17:29:31 +0800 Subject: [PATCH 2/6] energy changed --- energy/src/energy/run.cpp | 2 -- others/include/config/setconfig.h | 22 +++++++++++----------- 2 files changed, 11 insertions(+), 13 deletions(-) diff --git a/energy/src/energy/run.cpp b/energy/src/energy/run.cpp index 610ed61..9cf3006 100644 --- a/energy/src/energy/run.cpp +++ b/energy/src/energy/run.cpp @@ -107,8 +107,6 @@ void Energy::runBig(cv::Mat &gimbal_src) { getPredictPoint(target_point); getAimPoint(predict_point); judgeShootInGimbal(); -// cout << "yaw: " << yaw_rotation << '\t' << "pitch: " << pitch_rotation << '\t' << "shoot: " << shoot << endl; -// waitKey(0); sendEnergy(); } diff --git a/others/include/config/setconfig.h b/others/include/config/setconfig.h index 0768d1c..d0f71a5 100644 --- a/others/include/config/setconfig.h +++ b/others/include/config/setconfig.h @@ -32,34 +32,34 @@ #define ENERGY_CAMERA_GAIN (20) #endif #ifndef SMALL_YAW_AIM_KD - #define SMALL_YAW_AIM_KD (0) + #define SMALL_YAW_AIM_KD (0.35) #endif #ifndef SMALL_YAW_AIM_KP - #define SMALL_YAW_AIM_KP (4) + #define SMALL_YAW_AIM_KP (0.85) #endif #ifndef SMALL_PITCH_AIM_KD - #define SMALL_PITCH_AIM_KD (0) + #define SMALL_PITCH_AIM_KD (0.35) #endif #ifndef SMALL_PITCH_AIM_KP - #define SMALL_PITCH_AIM_KP (3.7) + #define SMALL_PITCH_AIM_KP (0.85) #endif #ifndef BIG_YAW_AIM_KD - #define BIG_YAW_AIM_KD (0) + #define BIG_YAW_AIM_KD (0.35) #endif #ifndef BIG_YAW_AIM_KP - #define BIG_YAW_AIM_KP (6.5) + #define BIG_YAW_AIM_KP (0.85) #endif #ifndef BIG_YAW_AIM_KI - #define BIG_YAW_AIM_KI (0.1) + #define BIG_YAW_AIM_KI (0) #endif #ifndef BIG_PITCH_AIM_KD - #define BIG_PITCH_AIM_KD (0) + #define BIG_PITCH_AIM_KD (0.35) #endif #ifndef BIG_PITCH_AIM_KP - #define BIG_PITCH_AIM_KP (6.5) + #define BIG_PITCH_AIM_KP (0.85) #endif #ifndef BIG_PITCH_AIM_KI - #define BIG_PITCH_AIM_KI (0.1) + #define BIG_PITCH_AIM_KI (0) #endif #ifndef COMPENSATE_YAW #define COMPENSATE_YAW (5) @@ -68,7 +68,7 @@ #define COMPENSATE_PITCH (74) #endif #ifndef EXTRA_DELTA_X - #define EXTRA_DELTA_X (10) + #define EXTRA_DELTA_X (0) #endif #ifndef EXTRA_DELTA_Y #define EXTRA_DELTA_Y (10) From c953a99d349d0e4d7deb545fa12c8347bb83d809 Mon Sep 17 00:00:00 2001 From: sun Date: Mon, 5 Aug 2019 17:42:58 +0800 Subject: [PATCH 3/6] energy changed --- energy/src/energy/clear/energy_init.cpp | 3 +-- main.cpp | 4 ++-- 2 files changed, 3 insertions(+), 4 deletions(-) diff --git a/energy/src/energy/clear/energy_init.cpp b/energy/src/energy/clear/energy_init.cpp index afd640d..c518df5 100644 --- a/energy/src/energy/clear/energy_init.cpp +++ b/energy/src/energy/clear/energy_init.cpp @@ -82,8 +82,7 @@ void Energy::initEnergy() { void Energy::initEnergyPartParam() { // gimbal_energy_part_param_.GRAY_THRESH = 120;//home // gimbal_energy_part_param_.GRAY_THRESH = 200;//official -// gimbal_energy_part_param_.GRAY_THRESH =180;//single game - gimbal_energy_part_param_.GRAY_THRESH =100;//game + gimbal_energy_part_param_.GRAY_THRESH =180;//game gimbal_energy_part_param_.SPLIT_GRAY_THRESH = 180; gimbal_energy_part_param_.FAN_GRAY_THRESH = 75; gimbal_energy_part_param_.ARMOR_GRAY_THRESH = 80; diff --git a/main.cpp b/main.cpp index 850a291..bef8e67 100644 --- a/main.cpp +++ b/main.cpp @@ -66,8 +66,8 @@ int main(int argc, char *argv[]) { video_gimbal = new CameraWrapper(ARMOR_CAMERA_GAIN, 2/*, "armor"*/); video_chassis = new CameraWrapper(ENERGY_CAMERA_GAIN, 2/*, "energy"*/); } else { - video_gimbal = new VideoWrapper("/home/sun/桌面/shiying/blue_1.avi"); - video_chassis = new VideoWrapper("/home/sun/桌面/shiying/blue_1.avi"); + video_gimbal = new VideoWrapper("/home/sun/桌面/shiying/18.avi"); + video_chassis = new VideoWrapper("/home/sun/桌面/shiying/18.avi"); } if (video_gimbal->init()) { LOGM("video_gimbal source initialization successfully."); From 690e2a92242b71b8fc33bcb4d0596a0fcf8d022f Mon Sep 17 00:00:00 2001 From: sun Date: Mon, 5 Aug 2019 17:43:17 +0800 Subject: [PATCH 4/6] energy changed --- main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/main.cpp b/main.cpp index bef8e67..a5e31d2 100644 --- a/main.cpp +++ b/main.cpp @@ -30,7 +30,7 @@ using namespace std; McuData mcu_data = { // 单片机端回传结构体 0, // 当前云台yaw角 0, // 当前云台pitch角 - BIG_ENERGY_STATE, // 当前状态，自瞄-大符-小符 + ARMOR_STATE, // 当前状态，自瞄-大符-小符 0, // 云台角度标记位 1, // 是否启用数字识别 ENEMY_RED, // 敌方颜色 From acfc1e8701e0e8931dc9b077f4694070f4091b6c Mon Sep 17 00:00:00 2001 From: sun Date: Mon, 5 Aug 2019 19:33:00 +0800 Subject: [PATCH 5/6] energy changed --- energy/src/energy/run.cpp | 2 ++ energy/src/energy/send/send.cpp | 31 ++++++++++++++++--------------- main.cpp | 2 +- 3 files changed, 19 insertions(+), 16 deletions(-) diff --git a/energy/src/energy/run.cpp b/energy/src/energy/run.cpp index 9cf3006..0e606f7 100644 --- a/energy/src/energy/run.cpp +++ b/energy/src/energy/run.cpp @@ -107,6 +107,8 @@ void Energy::runBig(cv::Mat &gimbal_src) { getPredictPoint(target_point); getAimPoint(predict_point); judgeShootInGimbal(); + cout << "yaw: " << yaw_rotation << '\t' << "pitch: " << pitch_rotation << '\t' << "shoot: " << shoot << endl; + waitKey(0); sendEnergy(); } diff --git a/energy/src/energy/send/send.cpp b/energy/src/energy/send/send.cpp index f310129..8da11de 100644 --- a/energy/src/energy/send/send.cpp +++ b/energy/src/energy/send/send.cpp @@ -18,35 +18,36 @@ void Energy::sendEnergy() { if (camera_cnt == 1) { sum_yaw += yaw_rotation; sum_pitch += pitch_rotation; - MINMAX(sum_yaw, -100, 100); - MINMAX(sum_pitch, -100, 100); + float yaw_I_component = BIG_YAW_AIM_KI * sum_yaw; + float pitch_I_component = BIG_PITCH_AIM_KI * sum_pitch; + MINMAX(yaw_I_component, -2, 2); + MINMAX(pitch_I_component, -2, 2); double tmp_yaw = yaw_rotation; double tmp_pitch = pitch_rotation; yaw_rotation = BIG_YAW_AIM_KP * yaw_rotation + BIG_YAW_AIM_KI * sum_yaw + BIG_YAW_AIM_KD * (yaw_rotation - last_yaw); pitch_rotation = BIG_PITCH_AIM_KP * pitch_rotation + BIG_PITCH_AIM_KI * sum_pitch + BIG_PITCH_AIM_KD * (pitch_rotation - last_pitch); - MINMAX(yaw_rotation, -10, 10); + MINMAX(yaw_rotation, -6, 6); + MINMAX(pitch_rotation, -6, 6); last_yaw = tmp_yaw; last_pitch = tmp_pitch; } else if (is_chassis) { - sum_yaw += yaw_rotation - mcu_data.curr_yaw; - sum_pitch += pitch_rotation - mcu_data.curr_pitch; - double tmp_yaw = yaw_rotation; - double tmp_pitch = pitch_rotation; - yaw_rotation = BIG_YAW_AIM_KP * (yaw_rotation - mcu_data.curr_yaw) + BIG_YAW_AIM_KI * sum_yaw; - pitch_rotation = BIG_PITCH_AIM_KP * (pitch_rotation - mcu_data.curr_pitch) + BIG_PITCH_AIM_KI * sum_pitch; - last_yaw = tmp_yaw; - last_pitch = tmp_pitch; +// sum_yaw += yaw_rotation - mcu_data.curr_yaw; +// sum_pitch += pitch_rotation - mcu_data.curr_pitch; +// double tmp_yaw = yaw_rotation; +// double tmp_pitch = pitch_rotation; +// yaw_rotation = BIG_YAW_AIM_KP * (yaw_rotation - mcu_data.curr_yaw) + BIG_YAW_AIM_KI * sum_yaw; +// pitch_rotation = BIG_PITCH_AIM_KP * (pitch_rotation - mcu_data.curr_pitch) + BIG_PITCH_AIM_KI * sum_pitch; +// last_yaw = tmp_yaw; +// last_pitch = tmp_pitch; } } else if (is_small){ - sum_yaw += yaw_rotation; - sum_pitch += pitch_rotation; - MINMAX(sum_yaw, -100, 100); - MINMAX(sum_pitch, -100, 100); double tmp_yaw = yaw_rotation; double tmp_pitch = pitch_rotation; yaw_rotation = SMALL_YAW_AIM_KP * yaw_rotation + SMALL_YAW_AIM_KD * (yaw_rotation - last_yaw); pitch_rotation = SMALL_PITCH_AIM_KP * pitch_rotation + SMALL_PITCH_AIM_KD * (pitch_rotation - last_pitch); + MINMAX(yaw_rotation, -6, 6); + MINMAX(pitch_rotation, -6, 6); last_yaw = tmp_yaw; last_pitch = tmp_pitch; } diff --git a/main.cpp b/main.cpp index a5e31d2..bef8e67 100644 --- a/main.cpp +++ b/main.cpp @@ -30,7 +30,7 @@ using namespace std; McuData mcu_data = { // 单片机端回传结构体 0, // 当前云台yaw角 0, // 当前云台pitch角 - ARMOR_STATE, // 当前状态，自瞄-大符-小符 + BIG_ENERGY_STATE, // 当前状态，自瞄-大符-小符 0, // 云台角度标记位 1, // 是否启用数字识别 ENEMY_RED, // 敌方颜色 From 92493a4447245d91e4436ff7c5ff5e1c12de74b1 Mon Sep 17 00:00:00 2001 From: sun Date: Mon, 5 Aug 2019 19:34:34 +0800 Subject: [PATCH 6/6] energy changed --- energy/src/energy/run.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/energy/src/energy/run.cpp b/energy/src/energy/run.cpp index 0e606f7..9cf3006 100644 --- a/energy/src/energy/run.cpp +++ b/energy/src/energy/run.cpp @@ -107,8 +107,6 @@ void Energy::runBig(cv::Mat &gimbal_src) { getPredictPoint(target_point); getAimPoint(predict_point); judgeShootInGimbal(); - cout << "yaw: " << yaw_rotation << '\t' << "pitch: " << pitch_rotation << '\t' << "shoot: " << shoot << endl; - waitKey(0); sendEnergy(); }