diff --git a/energy/include/energy/energy.h b/energy/include/energy/energy.h index 5761530..d19c800 100644 --- a/energy/include/energy/energy.h +++ b/energy/include/energy/energy.h @@ -29,6 +29,7 @@ public: void run(cv::Mat &gimbal_src, cv::Mat &chassis_src); void run(cv::Mat &gimbal_src); + void runSmall(cv::Mat &gimbal_src); Serial &serial;//串口 void setEnergyInit();//设置能量机关初始化 @@ -51,7 +52,6 @@ private: bool energy_rotation_init;//若仍在判断风车旋转方向，则为true bool manual_mark;//若操作手进行过手动标定，则为true bool auto_mark;//云台完成自动对心，则置为true - bool shoot;//若为true，则要求主控板发弹 bool start_guess;//进入猜测状态的标志 bool change_target;//目标切换的标志 @@ -77,6 +77,7 @@ private: float center_delta_yaw, center_delta_pitch;//对心时相差的角度 float yaw_rotation, pitch_rotation;//云台yaw轴和pitch轴应该转到的角度 float origin_yaw, origin_pitch;//初始的云台角度设定值 + float shoot;//若为2，则要求主控板发弹 timeval time_start_guess; @@ -158,8 +159,9 @@ private: void getAllTargetArmorCenters();//记录所有目标装甲板中心坐标 void getRecentTargetArmorCenters();//记录近30帧目标装甲板中心坐标 - void judgeShoot();//判断是否可以发弹 - void JudgeMode();//判断大符还是小符 + void judgeMode();//判断大符还是小符 + void judgeShootInGimbal();//在云台坐标系中判断是否可以发弹 + void judgeShootInWorld();//在世界坐标系中判断是否可以发弹 bool isGuessingTimeout();//判断猜测模式是否超时（没打中） void splitBayerBG(cv::Mat src, cv::Mat &blue, cv::Mat &red);//拜耳阵列分离 diff --git a/energy/src/energy/clear/energy_init.cpp b/energy/src/energy/clear/energy_init.cpp index 93af8cf..ef9e893 100644 --- a/energy/src/energy/clear/energy_init.cpp +++ b/energy/src/energy/clear/energy_init.cpp @@ -21,7 +21,6 @@ void Energy::initEnergy() { energy_rotation_init = true; manual_mark = false; auto_mark = false; - shoot = false; start_guess = false; change_target = false; @@ -41,14 +40,13 @@ void Energy::initEnergy() { last_target_polar_angle = -1000; guess_polar_angle = -1000; last_base_angle = -1000; - predict_rad = 20; + predict_rad = 30; attack_distance = ATTACK_DISTANCE; center_delta_yaw = 1000; center_delta_pitch = 1000; yaw_rotation = 0; pitch_rotation = 0; - origin_yaw = 0; - origin_pitch = 0; + shoot = 0; circle_center_point = Point(0, 0); target_point = Point(0, 0); @@ -69,8 +67,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 = 120;//home +// gimbal_energy_part_param_.GRAY_THRESH = 200;//official // gimbal_energy_part_param_.GRAY_THRESH = 225; gimbal_energy_part_param_.SPLIT_GRAY_THRESH = 230; gimbal_energy_part_param_.FAN_GRAY_THRESH = 75; @@ -142,8 +140,8 @@ void Energy::initEnergyPartParam() { -// chassis_energy_part_param_.GRAY_THRESH = 120;//home - chassis_energy_part_param_.GRAY_THRESH = 200;//official + chassis_energy_part_param_.GRAY_THRESH = 120;//home +// chassis_energy_part_param_.GRAY_THRESH = 200;//official // chassis_energy_part_param_.GRAY_THRESH = 225; chassis_energy_part_param_.SPLIT_GRAY_THRESH = 230; chassis_energy_part_param_.FAN_GRAY_THRESH = 75; @@ -204,7 +202,7 @@ void Energy::initEnergyPartParam() { chassis_energy_part_param_.FLOW_STRIP_CONTOUR_LENGTH_MIN = 55; chassis_energy_part_param_.FLOW_STRIP_CONTOUR_LENGTH_MAX = 95; chassis_energy_part_param_.FLOW_STRIP_CONTOUR_WIDTH_MIN = 8; - chassis_energy_part_param_.FLOW_STRIP_CONTOUR_WIDTH_MAX = 20; + chassis_energy_part_param_.FLOW_STRIP_CONTOUR_WIDTH_MAX = 25; chassis_energy_part_param_.FLOW_STRIP_CONTOUR_HW_RATIO_MAX = 12; // chassis_energy_part_param_.FLOW_STRIP_CONTOUR_HW_RATIO_MIN = 4; chassis_energy_part_param_.FLOW_STRIP_CONTOUR_HW_RATIO_MIN = 2.7; diff --git a/energy/src/energy/find/energy_finder.cpp b/energy/src/energy/find/energy_finder.cpp index 207cd64..100a16b 100644 --- a/energy/src/energy/find/energy_finder.cpp +++ b/energy/src/energy/find/energy_finder.cpp @@ -69,6 +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); // findContours(src_bin, armor_contours_external, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_NONE); // for (int i = 0; i < armor_contours_external.size(); i++)//去除外轮廓 diff --git a/energy/src/energy/get/origin_get.cpp b/energy/src/energy/get/origin_get.cpp index 83ddcff..6dfb8e5 100644 --- a/energy/src/energy/get/origin_get.cpp +++ b/energy/src/energy/get/origin_get.cpp @@ -13,18 +13,20 @@ using namespace cv; // --------------------------------------------------------------------------------------------------------------------- bool Energy::getOrigin() { if (!auto_mark && !manual_mark) { - double dx = -(circle_center_point.x - 320); - double dy = -(circle_center_point.y - 240); - center_delta_yaw = atan(dx / FOCUS_PIXAL) * 180 / PI; - center_delta_pitch = atan(dy / FOCUS_PIXAL) * 180 / PI; - if (abs(center_delta_pitch) > 0.3 || abs(center_delta_pitch) > 0.3) { - sendTarget(serial, center_delta_yaw, center_delta_pitch, false); + double dx = -(circle_center_point.x - 320 - 10); + double dy = -(circle_center_point.y - 240 - 22); + center_delta_yaw = static_cast(atan(dx / FOCUS_PIXAL) * 180 / PI); + center_delta_pitch = static_cast(atan(dy / FOCUS_PIXAL) * 180 / PI); + 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, 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_mode.cpp b/energy/src/energy/judge/judge_mode.cpp index 1eb5ed4..87bfa12 100644 --- a/energy/src/energy/judge/judge_mode.cpp +++ b/energy/src/energy/judge/judge_mode.cpp @@ -12,7 +12,7 @@ using namespace cv; //---------------------------------------------------------------------------------------------------------------------- // 此函数用于判断大小符 // --------------------------------------------------------------------------------------------------------------------- -void Energy::JudgeMode() { +void Energy::judgeMode() { getRecentTargetArmorCenters(); if (recent_target_armor_centers.size() < 30) { return; diff --git a/energy/src/energy/judge/judge_shoot.cpp b/energy/src/energy/judge/judge_shoot.cpp index d456af4..e2094f6 100644 --- a/energy/src/energy/judge/judge_shoot.cpp +++ b/energy/src/energy/judge/judge_shoot.cpp @@ -9,18 +9,33 @@ using namespace std; using namespace cv; - //---------------------------------------------------------------------------------------------------------------------- -// 此函数用于判断是否可以发弹 +// 此函数用于判断云台坐标系下是否可以发弹 // --------------------------------------------------------------------------------------------------------------------- -void Energy::judgeShoot(){ +void Energy::judgeShootInGimbal(){ if (abs(yaw_rotation - mcuData.curr_yaw) < 0.3 && fabs(pitch_rotation - mcuData.curr_pitch) < 0.3) { - shoot = true; + shoot = 4; is_predicting = false; is_guessing = true; start_guess = true; gettimeofday(&time_start_guess, NULL); LOGM(STR_CTR(WORD_LIGHT_RED, "Start Guessing!")); } else - shoot = false; + shoot = 2; +} + + +//---------------------------------------------------------------------------------------------------------------------- +// 此函数用于判断世界坐标系下是否可以发弹 +// --------------------------------------------------------------------------------------------------------------------- +void Energy::judgeShootInWorld(){ + if (abs(yaw_rotation) < 0.3 && fabs(pitch_rotation) < 0.3) { + shoot = 4; + 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; } diff --git a/energy/src/energy/run.cpp b/energy/src/energy/run.cpp index 17d2a87..d8393ab 100644 --- a/energy/src/energy/run.cpp +++ b/energy/src/energy/run.cpp @@ -16,12 +16,11 @@ void Energy::run(cv::Mat &gimbal_src, cv::Mat &chassis_src) { if (chassis_src.empty()) run(gimbal_src);//仅拥有云台摄像头则调用单摄像头的run函数 else if (is_gimbal) { -// energy_part_param_ = chassis_energy_part_param_; - energy_part_param_ = chassis_energy_part_param_; + energy_part_param_ = gimbal_energy_part_param_; clearAll(); initImage(gimbal_src); - findFans(gimbal_src); - showFans("fan",gimbal_src); +// findFans(gimbal_src); +// showFans("fan",gimbal_src); if (findArmors(gimbal_src) < 1)return; if (show_energy)showArmors("armor", gimbal_src); @@ -37,7 +36,6 @@ void Energy::run(cv::Mat &gimbal_src, cv::Mat &chassis_src) { initEnergy(); destroyAllWindows(); } else if (is_chassis) { -// energy_part_param_ = chassis_energy_part_param_; energy_part_param_ = chassis_energy_part_param_; clearAll(); initImage(chassis_src); @@ -47,16 +45,17 @@ void Energy::run(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; + if (!findFlowStripFan(chassis_src))return; if (!findTargetInFlowStripFan()) return; if (!findCenterROI(chassis_src))return; if (show_energy)showFlowStripFan("strip", chassis_src); if (!findCenterR(chassis_src))return; if (show_energy)showCenterR("R", chassis_src); getTargetPolarAngle(); + changeTarget(); - JudgeMode(); - if (energy_mode_init)return; +// judgeMode(); +// if (energy_mode_init)return; if (is_big && energy_rotation_init) { initRotation(); return; @@ -64,7 +63,7 @@ void Energy::run(cv::Mat &gimbal_src, cv::Mat &chassis_src) { if (is_predicting) { getPredictPoint(target_point); gimbalRotation(); - judgeShoot(); + judgeShootInGimbal(); sendTarget(serial, yaw_rotation, pitch_rotation, shoot); } else if (is_guessing && stayGuessing()) { findFans(chassis_src); @@ -74,7 +73,7 @@ void Energy::run(cv::Mat &gimbal_src, cv::Mat &chassis_src) { if (show_energy)showGuessTarget("guess", chassis_src); getPredictPoint(guess_point); gimbalRotation(); - sendTarget(serial, yaw_rotation, pitch_rotation, false); + sendTarget(serial, yaw_rotation, pitch_rotation, 5); } } } @@ -102,8 +101,8 @@ void Energy::run(cv::Mat &gimbal_src) { if (show_energy)showCenterR("R", gimbal_src); changeTarget(); getTargetPolarAngle(); - JudgeMode(); - if (energy_mode_init)return; +// judgeMode(); +// if (energy_mode_init)return; if (!getOrigin())return; if (is_big && energy_rotation_init) { initRotation(); @@ -112,7 +111,7 @@ void Energy::run(cv::Mat &gimbal_src) { if (is_predicting) { getPredictPoint(target_point); gimbalRotation(); - judgeShoot(); + judgeShootInGimbal(); sendTarget(serial, yaw_rotation, pitch_rotation, shoot); } else if (is_guessing && stayGuessing()) { findFans(gimbal_src); @@ -122,6 +121,71 @@ void Energy::run(cv::Mat &gimbal_src) { if (show_energy)showGuessTarget("guess", gimbal_src); getPredictPoint(guess_point); gimbalRotation(); - sendTarget(serial, yaw_rotation, pitch_rotation, false); + sendTarget(serial, yaw_rotation, pitch_rotation, 5); } + } + + + +//---------------------------------------------------------------------------------------------------------------------- +// 此函数为小能量机关模式主控制流函数，击打小符只需要拥有云台摄像头 +// --------------------------------------------------------------------------------------------------------------------- +void Energy::runSmall(cv::Mat &gimbal_src){ + energy_part_param_ = gimbal_energy_part_param_; + clearAll(); + initImage(gimbal_src); + + if (show_process)imshow("bin", gimbal_src); + if (findArmors(gimbal_src) < 1)return; + if (show_energy)showArmors("armor", gimbal_src); + if (!findFlowStripFan(gimbal_src))return; + if (!findTargetInFlowStripFan()) return; + if (show_energy)showFlowStripFan("strip", gimbal_src); + + changeTarget(); + getTargetPolarAngle(); + if (!getOrigin())return; + + getPredictPoint(target_point); + gimbalRotation(); + getAimPoint(target_point); + + if (is_predicting) { + getPredictPoint(target_point); + getAimPoint(target_point); + judgeShootInWorld(); + 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(target_point); + sendTarget(serial, yaw_rotation, pitch_rotation, 5); + } + +} + +//getPredictPoint(target_point); +//gimbalRotation(); +//static bool k = false; +//cout<<"delta yaw: "<( ARMOR_CENTER_TO_CYCLE_CENTER * x_0 / r_0); point_2.y = static_cast( ARMOR_CENTER_TO_CYCLE_CENTER * y_0 / r_0); } diff --git a/main.cpp b/main.cpp index 56caf4a..a95dc8e 100644 --- a/main.cpp +++ b/main.cpp @@ -30,7 +30,7 @@ using namespace std; mcu_data mcuData = { // 单片机端回传结构体 0, // 当前云台yaw角 0, // 当前云台pitch角 - ARMOR_STATE, // 当前状态，自瞄-大符-小符 + BIG_ENERGY_STATE, // 当前状态，自瞄-大符-小符 0, // 云台角度标记位 1, // 是否启用数字识别 ENEMY_RED, // 敌方颜色 @@ -64,10 +64,8 @@ int main(int argc, char *argv[]) { video_gimbal = new CameraWrapper(0/*, "armor"*/); video_chassis = new CameraWrapper(1/*, "energy"*/); } else { -// video_gimbal = new VideoWrapper("/home/sjturm/Desktop/videos/147.avi"); -// video_chassis = new VideoWrapper("/home/sjturm/Desktop/videos/147.avi"); - video_gimbal = new VideoWrapper("/home/sjturm/Desktop/RM2019 能量机关视频/大能量机关（蓝+开灯）.mov"); - video_chassis = new VideoWrapper("/home/sjturm/Desktop/RM2019 能量机关视频/大能量机关（蓝+开灯）.mov"); + video_gimbal = new VideoWrapper("/home/sun/项目/energy_video/148.avi"); + video_chassis = new VideoWrapper("/home/sun/项目/energy_video/148.avi"); } if (video_gimbal->init()) { LOGM("video_gimbal source initialization successfully."); @@ -105,7 +103,6 @@ int main(int argc, char *argv[]) { #endif } ok = checkReconnect(video_gimbal->read(gimbal_src)); - video_gimbal->read(gimbal_src); video_chassis->read(chassis_src); #ifdef GIMBAL_FLIP_MODE flip(gimbal_src, gimbal_src, GIMBAL_FLIP_MODE); @@ -113,8 +110,8 @@ int main(int argc, char *argv[]) { 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.run(gimbal_src, chassis_src); -// energy.run(gimbal_src); +// energy.run(gimbal_src, chassis_src); + energy.run(gimbal_src); last_state = mcuData.state;//更新上一帧状态 } else { // 自瞄模式 if (last_state != ARMOR_STATE) { @@ -133,7 +130,7 @@ int main(int argc, char *argv[]) { armorFinder.run(gimbal_src); }); } - cv::waitKey(1); +// cv::waitKey(1); }); } while (ok); delete video_gimbal; diff --git a/others/src/additions/additions.cpp b/others/src/additions/additions.cpp index fd2ae6c..77047ae 100644 --- a/others/src/additions/additions.cpp +++ b/others/src/additions/additions.cpp @@ -37,9 +37,19 @@ void uartReceive(Serial *pSerial) { pSerial->ReadData((uint8_t *) buffer, sizeof(mcuData)+1); if (buffer[sizeof(mcuData)] == '\n') { memcpy(&mcuData, buffer, sizeof(mcuData)); - LOGM("Get, state:%c, mark:%d!", mcuData.state, (int) mcuData.mark); +// LOGM("Get, state:%c, mark:%d!", mcuData.state, (int) mcuData.mark); +// LOGM("Get yaw: %f, pitch: %f!", mcuData.curr_yaw, mcuData.curr_pitch); +// static int t = time(nullptr); +// static int cnt = 0; +// if(time(nullptr) > t){ +// t = time(nullptr); +// LOGM("fps:%d", cnt); +// cnt = 0; +// }else{ +// cnt++; +// } }else{ - LOGW("corrupt data!"); +// LOGW("corrupt data!"); } } }