diff --git a/energy/include/energy/constant.h b/energy/include/energy/constant.h index 3deabeb..bdb82cf 100644 --- a/energy/include/energy/constant.h +++ b/energy/include/energy/constant.h @@ -11,7 +11,8 @@ const int SRC_WIDTH = 320; const int SRC_HEIGHT = 240; const int CLOCKWISE = 1; const int ANTICLOCKWISE = -1; -const float ATTACK_DISTANCE = 718.0;//cm +//const float ATTACK_DISTANCE = 718.0;//cm +const float ATTACK_DISTANCE = 750.0;//cm const double ARMOR_CENTER_TO_CYCLE_CENTER = 75.0;//cm diff --git a/energy/src/energy/clear/energy_init.cpp b/energy/src/energy/clear/energy_init.cpp index ef9e893..07a4703 100644 --- a/energy/src/energy/clear/energy_init.cpp +++ b/energy/src/energy/clear/energy_init.cpp @@ -40,7 +40,7 @@ void Energy::initEnergy() { last_target_polar_angle = -1000; guess_polar_angle = -1000; last_base_angle = -1000; - predict_rad = 30; + predict_rad = 25; attack_distance = ATTACK_DISTANCE; center_delta_yaw = 1000; center_delta_pitch = 1000; @@ -67,7 +67,7 @@ void Energy::initEnergy() { // 此函数对能量机关参数进行初始化 // --------------------------------------------------------------------------------------------------------------------- void Energy::initEnergyPartParam() { - gimbal_energy_part_param_.GRAY_THRESH = 120;//home + gimbal_energy_part_param_.GRAY_THRESH = 140;//home // gimbal_energy_part_param_.GRAY_THRESH = 200;//official // gimbal_energy_part_param_.GRAY_THRESH = 225; gimbal_energy_part_param_.SPLIT_GRAY_THRESH = 230; @@ -108,17 +108,17 @@ void Energy::initEnergyPartParam() { gimbal_energy_part_param_.CENTER_R_CONTOUR_AREA_RATIO_MIN = 0.7; gimbal_energy_part_param_.CENTER_R_CONTOUR_INTERSETION_AREA_MIN = 10; - gimbal_energy_part_param_.FLOW_STRIP_FAN_CONTOUR_AREA_MAX = 17000; - gimbal_energy_part_param_.FLOW_STRIP_FAN_CONTOUR_AREA_MIN = 0; + gimbal_energy_part_param_.FLOW_STRIP_FAN_CONTOUR_AREA_MAX = 3000; + gimbal_energy_part_param_.FLOW_STRIP_FAN_CONTOUR_AREA_MIN = 1000; gimbal_energy_part_param_.FLOW_STRIP_FAN_CONTOUR_LENGTH_MIN = 60; gimbal_energy_part_param_.FLOW_STRIP_FAN_CONTOUR_LENGTH_MAX = 100; gimbal_energy_part_param_.FLOW_STRIP_FAN_CONTOUR_WIDTH_MIN = 20; gimbal_energy_part_param_.FLOW_STRIP_FAN_CONTOUR_WIDTH_MAX = 50; gimbal_energy_part_param_.FLOW_STRIP_FAN_CONTOUR_HW_RATIO_MAX = 3; gimbal_energy_part_param_.FLOW_STRIP_FAN_CONTOUR_HW_RATIO_MIN = 1; - gimbal_energy_part_param_.FLOW_STRIP_FAN_CONTOUR_AREA_RATIO_MAX = 0.58; + gimbal_energy_part_param_.FLOW_STRIP_FAN_CONTOUR_AREA_RATIO_MAX = 0.65; gimbal_energy_part_param_.FLOW_STRIP_FAN_CONTOUR_AREA_RATIO_MIN = 0.34; - gimbal_energy_part_param_.FLOW_STRIP_FAN_NON_ZERO_RATE_MAX = 0.58; + gimbal_energy_part_param_.FLOW_STRIP_FAN_NON_ZERO_RATE_MAX = 0.65; gimbal_energy_part_param_.FLOW_STRIP_FAN_NON_ZERO_RATE_MIN = 0.34; // gimbal_energy_part_param_.FLOW_STRIP_FAN_NON_ZERO_RATE_MAX = 0.2; // gimbal_energy_part_param_.FLOW_STRIP_FAN_NON_ZERO_RATE_MIN = 0.08; @@ -131,7 +131,7 @@ void Energy::initEnergyPartParam() { gimbal_energy_part_param_.FLOW_STRIP_CONTOUR_WIDTH_MAX = 20; gimbal_energy_part_param_.FLOW_STRIP_CONTOUR_HW_RATIO_MAX = 12; // gimbal_energy_part_param_.FLOW_STRIP_CONTOUR_HW_RATIO_MIN = 4; - gimbal_energy_part_param_.FLOW_STRIP_CONTOUR_HW_RATIO_MIN = 3; + gimbal_energy_part_param_.FLOW_STRIP_CONTOUR_HW_RATIO_MIN = 2.7; gimbal_energy_part_param_.FLOW_STRIP_CONTOUR_AREA_RATIO_MIN = 0.5; gimbal_energy_part_param_.FLOW_STRIP_CONTOUR_INTERSETION_AREA_MIN = 117; diff --git a/energy/src/energy/get/aim_point_get.cpp b/energy/src/energy/get/aim_point_get.cpp index 65aa004..33bbf7b 100644 --- a/energy/src/energy/get/aim_point_get.cpp +++ b/energy/src/energy/get/aim_point_get.cpp @@ -12,8 +12,12 @@ using namespace cv; // 此函数通过自瞄逻辑击打目标点，用于大符的自动对心和小符直接打击 // --------------------------------------------------------------------------------------------------------------------- void Energy::getAimPoint(cv::Point target_point) { +//五号车 double dx = -(target_point.x - 320 - 10); double dy = -(target_point.y - 240 - 22); +//四号车 +// double dx = -(target_point.x - 320 + 16); +// double dy = -(target_point.y - 240 - 82); yaw_rotation = atan(dx / FOCUS_PIXAL) * 180 / PI; pitch_rotation = atan(dy / FOCUS_PIXAL) * 180 / PI; } diff --git a/energy/src/energy/judge/judge_contour.cpp b/energy/src/energy/judge/judge_contour.cpp index 2fa35ec..31b115a 100644 --- a/energy/src/energy/judge/judge_contour.cpp +++ b/energy/src/energy/judge/judge_contour.cpp @@ -143,6 +143,7 @@ bool Energy::isValidFlowStripFanContour(cv::Mat &src, const vector &f return false; //选区面积大小不合适 } +// cout << "area: " << cur_contour_area << endl; RotatedRect cur_rect = minAreaRect(flow_strip_fan_contour); Size2f cur_size = cur_rect.size; float length = cur_size.height > cur_size.width ? cur_size.height : cur_size.width;//将矩形的长边设置为长 @@ -151,11 +152,11 @@ bool Energy::isValidFlowStripFanContour(cv::Mat &src, const vector &f || width < energy_part_param_.FLOW_STRIP_FAN_CONTOUR_WIDTH_MIN || length > energy_part_param_.FLOW_STRIP_FAN_CONTOUR_LENGTH_MAX || width > energy_part_param_.FLOW_STRIP_FAN_CONTOUR_WIDTH_MAX) { - //cout<<"length width fail."< energy_part_param_.FLOW_STRIP_FAN_CONTOUR_HW_RATIO_MAX || length_width_ratio < energy_part_param_.FLOW_STRIP_FAN_CONTOUR_HW_RATIO_MIN) { @@ -166,6 +167,7 @@ bool Energy::isValidFlowStripFanContour(cv::Mat &src, const vector &f // cout << cur_contour_area / cur_size.area() << endl; if (cur_contour_area / cur_size.area() < energy_part_param_.FLOW_STRIP_FAN_CONTOUR_AREA_RATIO_MIN || cur_contour_area / cur_size.area() > energy_part_param_.FLOW_STRIP_FAN_CONTOUR_AREA_RATIO_MAX) { +// cout << "rate: " << cur_contour_area / cur_size.area() << endl; return false; } // 轮廓对矩形的面积占有率不合适 @@ -205,6 +207,7 @@ bool Energy::isValidFlowStripContour(const vector &flow_strip_contour return false; //矩形边长不合适 } +// cout << length << '\t' << width << endl; float length_width_ratio = length / width;//计算矩形长宽比 if (length_width_ratio > energy_part_param_.FLOW_STRIP_CONTOUR_HW_RATIO_MAX || length_width_ratio < energy_part_param_.FLOW_STRIP_CONTOUR_HW_RATIO_MIN) { diff --git a/energy/src/energy/judge/judge_shoot.cpp b/energy/src/energy/judge/judge_shoot.cpp index e2094f6..c4a50bd 100644 --- a/energy/src/energy/judge/judge_shoot.cpp +++ b/energy/src/energy/judge/judge_shoot.cpp @@ -10,10 +10,10 @@ using namespace cv; //---------------------------------------------------------------------------------------------------------------------- -// 此函数用于判断云台坐标系下是否可以发弹 +// 此函数用于判断世界坐标系下是否可以发弹 // --------------------------------------------------------------------------------------------------------------------- -void Energy::judgeShootInGimbal(){ - if (abs(yaw_rotation - mcuData.curr_yaw) < 0.3 && fabs(pitch_rotation - mcuData.curr_pitch) < 0.3) { +void Energy::judgeShootInWorld(){ + if (abs(yaw_rotation - mcuData.curr_yaw) < 0.8 && fabs(pitch_rotation - mcuData.curr_pitch) < 0.8) { shoot = 4; is_predicting = false; is_guessing = true; @@ -26,9 +26,9 @@ void Energy::judgeShootInGimbal(){ //---------------------------------------------------------------------------------------------------------------------- -// 此函数用于判断世界坐标系下是否可以发弹 +// 此函数用于判断云台坐标系下是否可以发弹 // --------------------------------------------------------------------------------------------------------------------- -void Energy::judgeShootInWorld(){ +void Energy::judgeShootInGimbal(){ if (abs(yaw_rotation) < 0.3 && fabs(pitch_rotation) < 0.3) { shoot = 4; is_predicting = false; @@ -38,4 +38,11 @@ void Energy::judgeShootInWorld(){ LOGM(STR_CTR(WORD_LIGHT_RED, "Start Guessing!")); } else shoot = 2; + + static float sum_yaw = 0; + static float sum_pitch = 0; + sum_yaw += yaw_rotation; + sum_pitch += pitch_rotation; + yaw_rotation = 2.5 * yaw_rotation + 0.1 * sum_yaw; + pitch_rotation = 2.5 * pitch_rotation + 0.1 * sum_pitch; } diff --git a/energy/src/energy/judge/judge_time.cpp b/energy/src/energy/judge/judge_time.cpp index dea7bfc..2fde1ce 100644 --- a/energy/src/energy/judge/judge_time.cpp +++ b/energy/src/energy/judge/judge_time.cpp @@ -17,5 +17,5 @@ bool Energy::isGuessingTimeout() { timeval cur_time; gettimeofday(&cur_time, NULL); return (cur_time.tv_sec - time_start_guess.tv_sec) * 1000.0 + - (cur_time.tv_usec - time_start_guess.tv_usec) / 1000.0 > 500; + (cur_time.tv_usec - time_start_guess.tv_usec) / 1000.0 > 1000; }; \ No newline at end of file diff --git a/energy/src/energy/run.cpp b/energy/src/energy/run.cpp index 0623ec3..f656a4a 100644 --- a/energy/src/energy/run.cpp +++ b/energy/src/energy/run.cpp @@ -63,7 +63,7 @@ void Energy::runBig(cv::Mat &gimbal_src, cv::Mat &chassis_src) { if (is_predicting) { getPredictPoint(target_point); gimbalRotation(); - judgeShootInGimbal(); + judgeShootInWorld(); sendTarget(serial, yaw_rotation, pitch_rotation, shoot); } else if (is_guessing && stayGuessing()) { findFans(chassis_src); @@ -104,14 +104,17 @@ void Energy::runBig(cv::Mat &gimbal_src) { // judgeMode(); // if (energy_mode_init)return; if (!getOrigin())return; - if (is_big && energy_rotation_init) { + if (energy_rotation_init) { initRotation(); return; } + if (is_predicting) { getPredictPoint(target_point); - gimbalRotation(); + 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); @@ -120,10 +123,26 @@ void Energy::runBig(cv::Mat &gimbal_src) { guessTarget(); if (show_energy)showGuessTarget("guess", gimbal_src); getPredictPoint(guess_point); - gimbalRotation(); + getAimPoint(predict_point); sendTarget(serial, yaw_rotation, pitch_rotation, 5); } +// if (is_predicting) { +// getPredictPoint(target_point); +// gimbalRotation(); +// 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); +// gimbalRotation(); +// sendTarget(serial, yaw_rotation, pitch_rotation, 5); +// } + } @@ -143,17 +162,11 @@ void Energy::runSmall(cv::Mat &gimbal_src) { if (show_energy)showFlowStripFan("strip", gimbal_src); changeTarget(); - getTargetPolarAngle(); - if (!getOrigin())return; - - getPredictPoint(target_point); - gimbalRotation(); - getAimPoint(target_point); + cout << "target point: " << target_point << endl; if (is_predicting) { - getPredictPoint(target_point); getAimPoint(target_point); - judgeShootInWorld(); + judgeShootInGimbal(); sendTarget(serial, yaw_rotation, pitch_rotation, shoot); } else if (is_guessing && stayGuessing()) { findFans(gimbal_src); @@ -161,8 +174,7 @@ void Energy::runSmall(cv::Mat &gimbal_src) { if (save_mark)writeDownMark(); guessTarget(); if (show_energy)showGuessTarget("guess", gimbal_src); - getPredictPoint(guess_point); - getAimPoint(target_point); + getAimPoint(guess_point); sendTarget(serial, yaw_rotation, pitch_rotation, 5); } diff --git a/main.cpp b/main.cpp index 20ebc9d..ca68173 100644 --- a/main.cpp +++ b/main.cpp @@ -64,8 +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/sun/项目/energy_video/148.avi"); - video_chassis = new VideoWrapper("/home/sun/项目/energy_video/148.avi"); + video_gimbal = new VideoWrapper("/home/sun/项目/energy_video/gimble3.avi"); + video_chassis = new VideoWrapper("/home/sun/项目/energy_video/gimble3.avi"); } if (video_gimbal->init()) { LOGM("video_gimbal source initialization successfully."); @@ -91,7 +91,7 @@ int main(int argc, char *argv[]) { bool ok = true; cout << "start running" << endl; do { - CNT_TIME("Total", { +// CNT_TIME("Total", { if (mcuData.state == BIG_ENERGY_STATE) {//大能量机关模式 if (last_state != BIG_ENERGY_STATE) {//若上一帧不是大能量机关模式，即刚往完成切换，则需要初始化 destroyAllWindows(); @@ -145,7 +145,7 @@ int main(int argc, char *argv[]) { }); } // cv::waitKey(1); - }); +// }); } while (ok); delete video_gimbal; video_gimbal = nullptr; diff --git a/others/src/additions/additions.cpp b/others/src/additions/additions.cpp index 77047ae..19dd316 100644 --- a/others/src/additions/additions.cpp +++ b/others/src/additions/additions.cpp @@ -76,12 +76,12 @@ cv::VideoWriter initVideoWriter(const std::string &filename_prefix) { bool checkReconnect(bool is_camera_0_connect, bool is_camera_1_connect) { if (!is_camera_0_connect) { delete video_gimbal; - video_gimbal = new CameraWrapper(0, "armor"); + video_gimbal = new CameraWrapper(0/*, "armor"*/); is_camera_0_connect = video_gimbal->init(); } if (!is_camera_1_connect) { delete video_chassis; - video_chassis = new CameraWrapper(1, "energy"); + video_chassis = new CameraWrapper(1/*, "energy"*/); is_camera_1_connect = video_chassis->init(); } return is_camera_0_connect && is_camera_1_connect; @@ -90,7 +90,7 @@ bool checkReconnect(bool is_camera_0_connect, bool is_camera_1_connect) { bool checkReconnect(bool is_camera_connect) { if (!is_camera_connect) { delete video_gimbal; - video_gimbal = new CameraWrapper(0, "armor"); + video_gimbal = new CameraWrapper(0/*, "armor"*/); is_camera_connect = video_gimbal->init(); } return is_camera_connect;