From 1a20133ca1d59e520a1581193cec250bf2bf44ba Mon Sep 17 00:00:00 2001 From: sun Date: Fri, 5 Jul 2019 14:06:40 +0800 Subject: [PATCH] energy changed --- energy/include/energy/energy.h | 177 +++++++----------- energy/include/energy/param_struct_define.h | 10 - .../src/energy/calculate/cycle_calculate.cpp | 57 +----- energy/src/energy/calibrate/split.cpp | 2 +- energy/src/energy/energy.cpp | 15 +- energy/src/energy/find/cycle_finder.cpp | 89 --------- energy/src/energy/find/energy_finder.cpp | 157 ++++++++-------- energy/src/energy/find/target_finder.cpp | 50 +++-- energy/src/energy/get/gimble_rotation_get.cpp | 51 ++--- energy/src/energy/get/hit_point_get.cpp | 133 ------------- energy/src/energy/get/position_get.cpp | 44 +++-- energy/src/energy/param_init.cpp | 82 +++----- energy/src/energy/run.cpp | 75 ++------ .../send_target_by_uart.cpp | 17 +- energy/src/energy/show/show.cpp | 92 +++++---- energy/src/energy/tool/tool.cpp | 66 ++----- main.cpp | 4 +- 17 files changed, 325 insertions(+), 796 deletions(-) delete mode 100644 energy/src/energy/find/cycle_finder.cpp delete mode 100644 energy/src/energy/get/hit_point_get.cpp diff --git a/energy/include/energy/energy.h b/energy/include/energy/energy.h index 896f292..9aac222 100644 --- a/energy/include/energy/energy.h +++ b/energy/include/energy/energy.h @@ -23,130 +23,93 @@ using std::vector; class Energy { public: - Energy(Serial &u, uint8_t &color); - ~Energy(); + Energy(Serial &u, uint8_t &color);//构造函数，参数为串口和敌方颜色 + ~Energy();//默认析构函数 int run(cv::Mat &src); - - cv::Point2f uart_hit_point; - clock_t start; - Serial &serial; - -// void setAllyColor(int color); - void setRotation(int rotation); - void setEnergyRotationInit(); - - void extract(cv::Mat &src); - - void sendTargetByUart(float x, float y, float z); + Serial &serial;//串口 + void setEnergyRotationInit();//判断顺逆时针函数 + void extract(cv::Mat &src);//框取图像中的一块区域进行处理 + void sendTargetByUart(float x, float y, float z);//向主控板发送数据 private: + EnergyPartParam energy_part_param_;//能量机关的参数设置 + bool isMark;//若操作手正在手动标定，则为true + int fans_cnt;//图像中的扇叶个数 + int armors_cnt;//图像中的装甲板个数 + int centerRs_cnt;//图像中可能的风车中心字母R选区个数 + int last_fans_cnt;//上一帧的扇叶个数 + int last_armors_cnt;//上一帧的装甲板个数 + double radius;//大风车半径 + float target_polar_angle;//待击打装甲板的极坐标角度 + float last_target_polar_angle;//上一帧待击打装甲板的极坐标角度 + uint8_t &ally_color;//我方颜色 + int energy_rotation_direction;//风车旋转方向 + float attack_distance;//步兵与风车平面距离 + int send_cnt;//向主控板发送的数据总次数 + float yaw_rotation;//云台yaw轴应该转到的角度 + float pitch_rotation;//云台pitch轴应该转到的角度 + uint8_t last_mark;//用于记录上一帧操作手是否进行标定 + double predict_rad;//预测提前角 + bool energy_rotation_init;//若仍在判断风车旋转方向，则为true + int clockwise_rotation_init_cnt;//装甲板顺时针旋转次数 + int anticlockwise_rotation_init_cnt;//装甲板逆时针旋转次数 + float red_origin_yaw, red_origin_pitch;//红方的初始云台对心角度设定值 + float blue_origin_yaw, blue_origin_pitch;//蓝方的初始云台对心角度设定值 + float origin_yaw, origin_pitch;//初始的云台角度设定值 + float target_cnt;//用于记录寻找到的装甲板总数，该值变化则立即中断主控板发射进程，防止重复击打已点亮的装甲板 + bool save_new_mark;//若操作手进行过手动标定，则为true - EnergyPartParam energy_part_param_; - LiftHeight lift_height_; - bool isSendTarget; - bool isMark; - int fans_cnt; - int armors_cnt; - int centerRs_cnt; - int count; - int last_fans_cnt; - int last_armors_cnt; - double radius; - double target_position; - double last_target_position; - double last_hit_position; - float target_armor; - float last_target_armor; - uint8_t &ally_color; - int energy_part_rotation; - float attack_distance; - int send_cnt; - double rectified_focal_length; - double theta;//电机pitch轴应旋转的角度 - double phi;//电机yaw轴应旋转的角度 - float yaw_rotation; - float pitch_rotation; - uint8_t last_mark; - int position_mode; - int last_position_mode; - int isLeftVertexFound, isTopVertexFound, isRightVertexFound, isBottomVertexFound; - bool energy_rotation_init; - int clockwise_rotation_init_cnt; - int anticlockwise_rotation_init_cnt; - float red_origin_yaw, red_origin_pitch; - float blue_origin_yaw, blue_origin_pitch; - float origin_yaw, origin_pitch; - float target_cnt; - bool target_cnt_flag; - bool save_new_mark; + std::vector fans;//图像中所有扇叶 + std::vector armors;//图像中所有装甲板 + std::vector centerRs;//风车中心字母R的可能候选区 - std::vector fans; - std::vector armors; - std::vector centerRs; - // std::vector gimble_zero_points; + cv::Point circle_center_point;//风车圆心坐标 + cv::Point target_point;//目标装甲板中心坐标 + cv::Point predict_point;//预测的击打点坐标 + std::vectorfan_polar_angle;//当前帧所有扇叶的极坐标角度 + std::vectorarmor_polar_angle;//当前帧所有装甲板的极坐标角度 + std::vector all_armor_centers;//记录全部的装甲板中心，用于风车圆心和半径的计算 + cv::Mat src_blue, src_red, src_green;//通道分离中的三个图像通道 - cv::Point cycle_center; - cv::Point target_center; - cv::Point last_target_center; - cv::Point hit_point; - std::vectorfanPosition; - std::vectorarmorPosition; - std::vector Armor_center; - std::vector first_armor_centers; - std::vector all_armor_centers; - cv::Point left, right, top, bottom; - cv::Mat src_blue, src_red, src_green; - - void initEnergy(); - void initEnergyPartParam(); - void initRotation(); + void initEnergy();//能量机关初始化 + void initEnergyPartParam();//能量机关参数初始化 + void initRotation();//顺逆时针初始化 - int findFan(const cv::Mat src, int &last_fans_cnt); - int findArmor(const cv::Mat src, int &last_armors_cnt); - int findCenterR(const cv::Mat src); + int findFan(const cv::Mat src, int &last_fans_cnt);//寻找图中所有扇叶 + int findArmor(const cv::Mat src, int &last_armors_cnt);//寻找图中所有装甲板 + int findCenterR(const cv::Mat src);//寻找图中可能的风车中心字母R - void showFanContours(std::string windows_name, const cv::Mat &src, const std::vector &fans); - void showArmorContours(std::string windows_name, const cv::Mat &src, const std::vector &armors); - void showBothContours(std::string windows_name, const cv::Mat src); - void showCenterRContours(std::string windows_name, const cv::Mat src); + bool isValidFanContour(const vector fan_contour);//扇叶矩形尺寸要求 + bool isValidArmorContour(const vector armor_contour);//装甲板矩形尺寸要求 + bool isValidCenterRContour(const vector center_R_contour);//风车中心选区尺寸要求 - bool isValidFanContour(const vector fan_contour); - bool isValidArmorContour(const vector armor_contour); - bool isValidCenterRContour(const vector center_R_contour); + void showFanContours(std::string windows_name, const cv::Mat src);//显示扇叶 + void showArmorContours(std::string windows_name, const cv::Mat src);//显示装甲板 + void showBothContours(std::string windows_name, const cv::Mat src);//显示扇叶和装甲板 + void showCenterRContours(std::string windows_name, const cv::Mat src);//显示风车中心候选区R - void getFanPosition(std::vector &fanPosition, const std::vector &fans, cv::Point cycle_center, double radius); - void getArmorPosition(std::vector &armorPosition, const std::vector &armors, cv::Point cycle_center, double radius); - void getFirstArmorCenters(vector &armors, std::vector &first_armor_centers); - void getAllArmorCenters(); - void getPosition(cv::Point point, double &angle); + void getFanPosition();//获取扇叶极坐标角度 + void getArmorPosition();//获取装甲板极坐标角度 + void getAllArmorCenters();//记录所有装甲板中心坐标 - void cycleQuickCalculate(std::vector &first_armor_centers, cv::Point &cycle_center, double &radius); - void cycleDefaultCalculateConst(cv::Point &cycle_center, double &radius); - void cycleCalculate(); - void cycleLeastFit(); + void cycleLeastFit();//利用所有记录的装甲板中心最小二乘法计算圆心和半径 - void findTarget(const std::vectorfanPosition, const std::vectorarmorPosition, float &target_armor); + void findTarget();//获取目标装甲板的极坐标角度和装甲板中心坐标 - void findWholeCycle(const std::vector&first_armor_centers); + void rotate();//获取预测点位 + void stretch(cv::Point point_1, cv::Point2f &point_2);//将像素差转换为实际距离差 - void saveFourPoints(std::vector &FourPoints, cv::Point point_1, cv::Point point_2, cv::Point point_3, cv::Point point_4); - void savePoint2f(std::vector &point_save, cv::Point point); - double pointDistance(cv::Point point_1, cv::Point point_2); - void rotate(double rad, double radius, cv::Point center, cv::Point point_old, cv::Point &point_new); - void stretch(cv::Point point_1, cv::Point2f &point_2); - void cycle(cv::Point p1, cv::Point p2, cv::Point p3, cv::Point ¢er, double &radius); + void getPredictPoint();//获取预测点位 + bool changeTarget();//判断目标是否改变 + void changeMark();//操作手手动修改标定值 + void gimbleRotation();//计算云台旋转角度 - void getHitPoint(); - bool changeTarget(); - void changeMark(); - void gimbleRotation(); + void splitBayerBG(cv::Mat src, cv::Mat &blue, cv::Mat &red);//拜耳阵列分离 + void imagePreprocess(cv::Mat &src);//图像通道分离 - void splitBayerBG(cv::Mat &src, cv::Mat &blue, cv::Mat &red); - void imagePreprocess(cv::Mat &src); - - void StructingElementClose(cv::Mat &src,int length, int width); - void StructingElementErodeDilate(cv::Mat &src); + void StructingElementClose(cv::Mat &src,int length, int width);//闭运算 + void StructingElementErodeDilate(cv::Mat &src);//腐蚀和膨胀 }; diff --git a/energy/include/energy/param_struct_define.h b/energy/include/energy/param_struct_define.h index ff49a0a..8b05dc1 100644 --- a/energy/include/energy/param_struct_define.h +++ b/energy/include/energy/param_struct_define.h @@ -60,16 +60,6 @@ struct EnergyPartParam { float TWIN_ANGEL_MAX; }; -struct LiftHeight{ - float LIFT_0; - float LIFT_30; - float LIFT_60; - float LIFT_90; - float LIFT_minus_30; - float LIFT_minus_60; - float LIFT_minus_90; -}; - typedef struct GMAngle_t{ float yaw; float pitch; diff --git a/energy/src/energy/calculate/cycle_calculate.cpp b/energy/src/energy/calculate/cycle_calculate.cpp index 7593555..5b75bed 100644 --- a/energy/src/energy/calculate/cycle_calculate.cpp +++ b/energy/src/energy/calculate/cycle_calculate.cpp @@ -8,65 +8,26 @@ using std::cout; using std::endl; using std::vector; -void Energy::cycleQuickCalculate(std::vector &first_armor_centers, cv::Point &cycle_center, double &radius) { - //if (isCalibrated()) return; - int cur_size = static_cast(first_armor_centers.size()); - if (cur_size < 3)return; - cv::Point point_1, point_2, point_3; - point_1 = first_armor_centers.at(0); - point_2 = first_armor_centers.at(static_cast(cur_size - 1)); - point_3 = first_armor_centers.at(static_cast(cur_size / 2)); - //cout << point_1 << '\t' << point_2 << endl; - //cout << first_armor_centers.at(point_1) << '\t' << first_armor_centers.at(point_2) << endl; - cycle(point_1, point_2, point_3, cycle_center, radius); - cout << "The cycle center is: " << cycle_center << endl; - cout << "The radius is: " << radius << endl; -} -void Energy::cycleDefaultCalculateConst(cv::Point &cycle_center, double &radius) { - if (count >= 5) - return; - clock_t end; - double time_duration = 1.0, dt; - if (Armor_center.size() < 3) { - end = clock(); - dt = (end - start) / 1000000.00; - if (dt >= time_duration * count) { - getFirstArmorCenters(armors, Armor_center); - count++; - } - } - else { - cycleQuickCalculate(Armor_center, cycle_center, radius); - count++; - } -} - -void Energy::cycleCalculate(){ - cycle_center.x = (top.x + bottom.x)/2; - cycle_center.y = (left.y + right.y)/2; - radius = (right.x - left.x)/2; - cout << "The cycle center is: " << cycle_center << endl; - cout << "The radius is: " << radius << endl; -} +//---------------------------------------------------------------------------------------------------------------------- +// 此函数通过最小二乘法计算大风车圆心和半径 +// --------------------------------------------------------------------------------------------------------------------- void Energy::cycleLeastFit() { - cycle_center.x = 0; - cycle_center.y = 0; + circle_center_point.x = 0; + circle_center_point.y = 0; radius = 0.0f; if (all_armor_centers.size() < 3) { // cout<<"Cannot calculate a circle"<(all_armor_centers.size()); for (int i = 0; i < N; i++) { @@ -84,10 +45,8 @@ void Energy::cycleLeastFit() sum_x1y2 += x * y2; sum_x2y1 += x2 * y; } - double C, D, E, G, H; double a, b, c; - C = N * sum_x2 - sum_x * sum_x; D = N * sum_xy - sum_x * sum_y; E = N * sum_x3 + N * sum_x1y2 - (sum_x2 + sum_y2) * sum_x; @@ -96,11 +55,9 @@ void Energy::cycleLeastFit() a = (H * D - E * G) / (C * G - D * D); b = (H * C - E * D) / (D * D - G * C); c = -(a * sum_x + b * sum_y + sum_x2 + sum_y2) / N; - - cycle_center.x = static_cast(a / (-2)); - cycle_center.y = static_cast(b / (-2)); + circle_center_point.x = static_cast(a / (-2)); + circle_center_point.y = static_cast(b / (-2)); radius = sqrt(a * a + b * b - 4 * c) / 2; - // cout << "The cycle center is: " << cycle_center << endl; // cout << "The radius is: " << radius << endl; } diff --git a/energy/src/energy/calibrate/split.cpp b/energy/src/energy/calibrate/split.cpp index 427fb08..c4ab981 100644 --- a/energy/src/energy/calibrate/split.cpp +++ b/energy/src/energy/calibrate/split.cpp @@ -8,7 +8,7 @@ using std::cout; using std::endl; using std::vector; -void Energy::splitBayerBG(cv::Mat &src, cv::Mat &blue, cv::Mat &red) { +void Energy::splitBayerBG(cv::Mat src, cv::Mat &blue, cv::Mat &red) { uchar* data; uchar* bayer_data[2]; for (int i = 0; i < src.rows; ++i) { diff --git a/energy/src/energy/energy.cpp b/energy/src/energy/energy.cpp index 472c663..a25d52a 100644 --- a/energy/src/energy/energy.cpp +++ b/energy/src/energy/energy.cpp @@ -20,11 +20,11 @@ Energy::Energy(Serial &u, uint8_t &color):serial(u),ally_color(color), save_new_mark = true; - if(ally_color==ALLY_RED){ + if(ally_color == ALLY_RED){ origin_yaw = red_origin_yaw; origin_pitch = red_origin_pitch; } - else if(ally_color==ALLY_BLUE){ + else if(ally_color == ALLY_BLUE){ origin_yaw = blue_origin_yaw; origin_pitch = blue_origin_pitch; } @@ -35,22 +35,11 @@ Energy::Energy(Serial &u, uint8_t &color):serial(u),ally_color(color), Energy::~Energy() = default; -// -//void Energy::setAllyColor(int color) -//{ -// ally_color_ = color; -//} - -void Energy::setRotation(int rotation){ - energy_part_rotation = rotation; -} - void Energy::setEnergyRotationInit() { initEnergy(); initEnergyPartParam(); energy_rotation_init = true; - if(!save_new_mark){ FILE *fp = fopen(PROJECT_DIR"/Mark/mark.txt", "r"); if(fp){ diff --git a/energy/src/energy/find/cycle_finder.cpp b/energy/src/energy/find/cycle_finder.cpp deleted file mode 100644 index bff7ee9..0000000 --- a/energy/src/energy/find/cycle_finder.cpp +++ /dev/null @@ -1,89 +0,0 @@ -// -// Created by xixiliadorabarry on 19-3-23. -// - -#include "energy/energy.h" -#include - -using namespace cv; -using std::cout; -using std::endl; -using std::vector; - -void Energy::findWholeCycle(const std::vector&first_armor_centers) { - int cur_size = static_cast(first_armor_centers.size()); - //cout << "first armor centers' size: " << first_armor_centers.size() << endl; - if (cur_size == 0)return; - int vertex = 0; - for (int i = 1; i < cur_size - 1; ++i) - { - if (vertex == 4)break; - if (first_armor_centers.at(i).x >= first_armor_centers.at(i - 1).x && first_armor_centers.at(i).x >= first_armor_centers.at(i + 1).x) - { - if (isRightVertexFound == -1) { - vertex += 1; - isRightVertexFound = 1; - right = first_armor_centers.at(i); - cout << "right vertex: " << right << endl; - continue; - } - else if (right.x > first_armor_centers.at(i).x)continue; - else { - right = first_armor_centers.at(i); - continue; - } - } - if (first_armor_centers.at(i).x <= first_armor_centers.at(i - 1).x && first_armor_centers.at(i).x <= first_armor_centers.at(i + 1).x) - { - if (isLeftVertexFound == -1) { - vertex += 1; - isLeftVertexFound = 1; - left = first_armor_centers.at(i); - cout << "left vertex: " << left << endl; - continue; - } - else if (left.x < first_armor_centers.at(i).x)continue; - else { - left = first_armor_centers.at(i); - continue; - } - } - if (first_armor_centers.at(i).y <= first_armor_centers.at(i - 1).y && first_armor_centers.at(i).y <= first_armor_centers.at(i + 1).y) - { - if (isTopVertexFound == -1) { - vertex += 1; - isTopVertexFound = 1; - top = first_armor_centers.at(i); - cout << "top vertex: " << top << endl; - continue; - } - else if (top.y < first_armor_centers.at(i).y)continue; - else { - top = first_armor_centers.at(i); - continue; - } - - } - if (first_armor_centers.at(i).y >= first_armor_centers.at(i - 1).y && first_armor_centers.at(i).y >= first_armor_centers.at(i + 1).y) - { - if (isBottomVertexFound == -1) { - vertex += 1; - isBottomVertexFound = 1; - bottom = first_armor_centers.at(i); - cout << "bottom vertex: " << bottom << endl; - continue; - } - else if (bottom.y > first_armor_centers.at(i).y)continue; - else { - bottom = first_armor_centers.at(i); - continue; - } - } - } - - /*for (int k = 0; k < first_armor_centers.size(); ++k) { - cout << k << " : " << first_armor_centers.at(k) << '\t'; - float angle = static_cast(180 / PI * atan2(-1 * (first_armor_centers.at(k).y - 298), (first_armor_centers.at(k).x - 298))); - cout << angle << endl; - }*/ -} diff --git a/energy/src/energy/find/energy_finder.cpp b/energy/src/energy/find/energy_finder.cpp index 6aadb29..7f6ee9b 100644 --- a/energy/src/energy/find/energy_finder.cpp +++ b/energy/src/energy/find/energy_finder.cpp @@ -8,6 +8,8 @@ using std::cout; using std::endl; using std::vector; + + //---------------------------------------------------------------------------------------------------------------------- // 此函数用于寻找图像内所有的大风车扇叶 // --------------------------------------------------------------------------------------------------------------------- @@ -17,52 +19,49 @@ int Energy::findFan(const cv::Mat src, int &last_fans_cnt) { src_bin = src.clone(); // threshold(src, src_bin, energy_part_param_.FAN_GRAY_THRESH, 255, THRESH_BINARY); if(src.type() == CV_8UC3){ - cvtColor(src_bin, src_bin, CV_BGR2GRAY); + cvtColor(src_bin, src_bin, CV_BGR2GRAY);//若读取三通道视频文件，需转换为单通道 } - std::vector > fan_contours; - - StructingElementClose(src_bin,6,6); - + StructingElementClose(src_bin,6,6);//图像膨胀，防止图像断开并更方便寻找 // imshow("fan struct",src_bin); - findContours(src_bin, fan_contours, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_NONE); for (auto &fan_contour : fan_contours) { if (!isValidFanContour(fan_contour)) { continue; } + fans.emplace_back(fan_contour); - double cur_contour_area = contourArea(fan_contour); + /* double cur_contour_area = contourArea(fan_contour); RotatedRect cur_rect = minAreaRect(fan_contour); Size2f cur_size = cur_rect.size; -// -// cout<<"cur_contour_area: "< cur_size.width ? cur_size.height : cur_size.width; float width = cur_size.height < cur_size.width ? cur_size.height : cur_size.width; -// cout<<"fan area: "<20&&width>20){ -// cout<20&&width>20){ + cout<(fans.size()); - return -1; + return -1;//寻找到的扇叶比上一帧少，说明该帧有误，返回-1 } last_fans_cnt = static_cast(fans.size()); return static_cast(fans.size()); } + + //---------------------------------------------------------------------------------------------------------------------- // 此函数用于寻找图像内所有的大风车装甲板模块 // --------------------------------------------------------------------------------------------------------------------- @@ -72,12 +71,12 @@ int Energy::findArmor(const cv::Mat src, int &last_armors_cnt) { src_bin = src.clone(); // threshold(src, src_bin, energy_part_param_.ARMOR_GRAY_THRESH, 255, THRESH_BINARY); if(src.type() == CV_8UC3){ - cvtColor(src_bin, src_bin, CV_BGR2GRAY); + cvtColor(src_bin, src_bin, CV_BGR2GRAY);//若读取三通道视频文件，需转换为单通道 } std::vector > armor_contours; std::vector > armor_contours_external;//用总轮廓减去外轮廓，只保留内轮廓，除去流动条的影响。 - StructingElementErodeDilate(src_bin); + StructingElementErodeDilate(src_bin);//图像膨胀，防止图像断开并更方便寻找 // imshow("armor struct",src_bin); findContours(src_bin, armor_contours, CV_RETR_LIST, CV_CHAIN_APPROX_NONE); @@ -97,36 +96,35 @@ int Energy::findArmor(const cv::Mat src, int &last_armors_cnt) { } } } - for (auto &armor_contour : armor_contours) { if (!isValidArmorContour(armor_contour)) { continue; } + armors.emplace_back(armor_contour); - RotatedRect cur_rect = minAreaRect(armor_contour); + /* RotatedRect cur_rect = minAreaRect(armor_contour); Size2f cur_size = cur_rect.size; float length = cur_size.height > cur_size.width ? cur_size.height : cur_size.width; float width = cur_size.height < cur_size.width ? cur_size.height : cur_size.width; - -// if(length>10&&width>5){ -// armors.emplace_back(armor_contour); -// cout<<"armor area: "<10&&width>5){ + armors.emplace_back(armor_contour); + cout<<"armor area: "<(armors.size()); - return -1; + return -1;//寻找到的装甲板比上一帧少，说明该帧有误，返回-1 } last_armors_cnt = static_cast(armors.size()); return static_cast(armors.size()); } + + + //---------------------------------------------------------------------------------------------------------------------- // 此函数用于寻找图像内大风车中心字母“R” // --------------------------------------------------------------------------------------------------------------------- @@ -139,10 +137,8 @@ int Energy::findCenterR(const cv::Mat src) { cvtColor(src_bin, src_bin, CV_BGR2GRAY); } std::vector > center_R_contours; - StructingElementErodeDilate(src_bin); // 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) { @@ -150,24 +146,26 @@ int Energy::findCenterR(const cv::Mat src) { { continue; } + centerRs.emplace_back(center_R_contour); - RotatedRect cur_rect = minAreaRect(center_R_contour); + /*RotatedRect cur_rect = minAreaRect(center_R_contour); Size2f cur_size = cur_rect.size; float length = cur_size.height > cur_size.width ? cur_size.height : cur_size.width; float width = cur_size.height < cur_size.width ? cur_size.height : cur_size.width; -// if(length>10 && width>5){ -// centerRs.emplace_back(center_R_contour); -// cout<<"center R area: "<10 && width>5){ + centerRs.emplace_back(center_R_contour); + cout<<"center R area: "<(centerRs.size()); } + + + //---------------------------------------------------------------------------------------------------------------------- // 此函数用于判断找到的矩形候选区是否为扇叶 // --------------------------------------------------------------------------------------------------------------------- @@ -179,31 +177,35 @@ bool Energy::isValidFanContour(const vector fan_contour) { //cout< cur_size.width ? cur_size.height : cur_size.width; - float width = cur_size.height < cur_size.width ? cur_size.height : cur_size.width; + float length = cur_size.height > cur_size.width ? cur_size.height : cur_size.width;//将矩形的长边设置为长 + float width = cur_size.height < cur_size.width ? cur_size.height : cur_size.width;//将矩形的短边设置为宽 if (length < energy_part_param_.FAN_CONTOUR_LENGTH_MIN || width < energy_part_param_.FAN_CONTOUR_WIDTH_MIN || length > energy_part_param_.FAN_CONTOUR_LENGTH_MAX || width > energy_part_param_.FAN_CONTOUR_WIDTH_MAX) { - //cout<<"length width min fail."< energy_part_param_.FAN_CONTOUR_HW_RATIO_MAX || length_width_ratio < energy_part_param_.FAN_CONTOUR_HW_RATIO_MIN) { //cout<<"length width ratio fail."< armor_contour) { //cout< cur_size.width ? cur_size.height : cur_size.width; - float width = cur_size.height < cur_size.width ? cur_size.height : cur_size.width; - if (length < energy_part_param_.ARMOR_CONTOUR_LENGTH_MIN || width < energy_part_param_.ARMOR_CONTOUR_WIDTH_MIN) + float length = cur_size.height > cur_size.width ? cur_size.height : cur_size.width;//将矩形的长边设置为长 + float width = cur_size.height < cur_size.width ? cur_size.height : cur_size.width;//将矩形的短边设置为宽 + if (length < energy_part_param_.ARMOR_CONTOUR_LENGTH_MIN || width < energy_part_param_.ARMOR_CONTOUR_WIDTH_MIN || + length > energy_part_param_.ARMOR_CONTOUR_LENGTH_MAX||width>energy_part_param_.ARMOR_CONTOUR_WIDTH_MAX) { - //cout<<"length width min fail."< energy_part_param_.ARMOR_CONTOUR_LENGTH_MAX||width>energy_part_param_.ARMOR_CONTOUR_WIDTH_MAX) - { - //cout<<"length width max fail."< energy_part_param_.ARMOR_CONTOUR_HW_RATIO_MAX || length_width_ratio < energy_part_param_.ARMOR_CONTOUR_HW_RATIO_MIN) { //cout<<"length width ratio fail."< center_R_contour) { //cout< cur_size.width ? cur_size.height : cur_size.width; - float width = cur_size.height < cur_size.width ? cur_size.height : cur_size.width; - if (length < energy_part_param_.CENTER_R_CONTOUR_LENGTH_MIN || width < energy_part_param_.CENTER_R_CONTOUR_WIDTH_MIN) + float length = cur_size.height > cur_size.width ? cur_size.height : cur_size.width;//将矩形的长边设置为长 + float width = cur_size.height < cur_size.width ? cur_size.height : cur_size.width;//将矩形的短边设置为宽 + if (length < energy_part_param_.CENTER_R_CONTOUR_LENGTH_MIN || width < energy_part_param_.CENTER_R_CONTOUR_WIDTH_MIN + ||length > energy_part_param_.CENTER_R_CONTOUR_LENGTH_MAX ||width>energy_part_param_.CENTER_R_CONTOUR_WIDTH_MAX) { - //cout<<"length width min fail."< energy_part_param_.CENTER_R_CONTOUR_LENGTH_MAX||width>energy_part_param_.CENTER_R_CONTOUR_WIDTH_MAX) - { - //cout<<"length width max fail."< energy_part_param_.CENTER_R_CONTOUR_HW_RATIO_MAX || length_width_ratio < energy_part_param_.CENTER_R_CONTOUR_HW_RATIO_MIN) { //cout<<"length width ratio fail."<fanPosition, std::vectorarmorPosition, float &target_armor) { - if (fanPosition.size() >= armorPosition.size()) return; - if (armorPosition.size()==0)return; - if (fanPosition.size() == 0) { - target_armor = armorPosition.at(0); +void Energy::findTarget() { + if (fan_polar_angle.size() >= armor_polar_angle.size()) return;//扇叶多于装甲板，识别错误 + if (armor_polar_angle.empty())return;//找不到扇叶，识别错误 + if (fan_polar_angle.empty()) { + target_polar_angle = armor_polar_angle.at(0);//视野中没有扇叶，说明在击打第一个装甲板 for (const auto &armor : armors) { - target_center = armor.rect.center; -// cout<<"target center: "<::iterator it = fanPosition.begin(); it != fanPosition.end(); it++) { - cout << *it << endl; - }*/ - sort(armorPosition.begin(), armorPosition.end()); - /*for (vector::iterator it = armorPosition.begin(); it != armorPosition.end(); it++) { - cout << *it << endl; - }*/ + sort(fan_polar_angle.begin(), fan_polar_angle.end());//对扇叶的极坐标角度进行排序 + sort(armor_polar_angle.begin(), armor_polar_angle.end());//对装甲板的极坐标角度进行排序 int i, j = 0; - for (i = 0; i < fanPosition.size(); ++i) { - if (armorPosition.at(i) - fanPosition.at(j) < energy_part_param_.TWIN_ANGEL_MAX && armorPosition.at(i) - fanPosition.at(j) > -1 * energy_part_param_.TWIN_ANGEL_MAX) { + for (i = 0; i < fan_polar_angle.size(); ++i) { + if (armor_polar_angle.at(i) - fan_polar_angle.at(j) < energy_part_param_.TWIN_ANGEL_MAX + && armor_polar_angle.at(i) - fan_polar_angle.at(j) > -1 * energy_part_param_.TWIN_ANGEL_MAX) { j++; - continue; + continue;//若某个扇叶的极坐标角度与第j个装甲板的极坐标角度接近，则两者匹配成功 } else { - target_armor = armorPosition.at(j); + target_polar_angle = armor_polar_angle.at(j);//无法被匹配到的装甲板为待击打装甲板 for (const auto &armor : armors) { - float angle = static_cast(180 / PI * atan2(-1 * (armor.rect.center.y - cycle_center.y), (armor.rect.center.x - cycle_center.x))); - if(target_armor==angle){ - target_center = armor.rect.center; -// cout<<"target center: "<(180 / PI * atan2(-1 * (armor.rect.center.y - circle_center_point.y), + (armor.rect.center.x - circle_center_point.x))); + if(target_polar_angle == angle){ + target_point = armor.rect.center;//根据已经确定的目标装甲板极坐标角度，获得该装甲板的中心坐标 } } return; } } - target_armor = armorPosition.at(armorPosition.size() - 1); + target_polar_angle = armor_polar_angle.at(armor_polar_angle.size() - 1);//前几个扇叶都匹配到装甲板，则最后剩下的装甲板为目标 for (const auto &armor : armors) { - float angle = static_cast(180 / PI * atan2(-1 * (armor.rect.center.y - cycle_center.y), (armor.rect.center.x - cycle_center.x))); - if(target_armor == angle){ - target_center = armor.rect.center; -// cout<<"target center: "<(180 / PI * atan2(-1 * (armor.rect.center.y - circle_center_point.y), + (armor.rect.center.x - circle_center_point.x))); + if(target_polar_angle == angle){ + target_point = armor.rect.center;//根据已经确定的目标装甲板极坐标角度，获得该装甲板的中心坐标 } } diff --git a/energy/src/energy/get/gimble_rotation_get.cpp b/energy/src/energy/get/gimble_rotation_get.cpp index aaf68b2..c957e8c 100644 --- a/energy/src/energy/get/gimble_rotation_get.cpp +++ b/energy/src/energy/get/gimble_rotation_get.cpp @@ -10,6 +10,11 @@ using std::cout; using std::endl; using std::vector; + + +//---------------------------------------------------------------------------------------------------------------------- +// 此函数用于操作手手动标定 +// --------------------------------------------------------------------------------------------------------------------- void Energy::changeMark() { if (mcuData.mark == 0 && last_mark == 1) { last_mark = mcuData.mark; @@ -32,41 +37,15 @@ void Energy::changeMark() { } + +//---------------------------------------------------------------------------------------------------------------------- +// 此函数用于计算云台应当转到角度 +// --------------------------------------------------------------------------------------------------------------------- void Energy::gimbleRotation(){ - cv::Point2f real_hit_point; - stretch(hit_point, real_hit_point); - yaw_rotation = static_cast(180 / PI * atan2((attack_distance * tan(origin_yaw * PI / 180) - real_hit_point.x), attack_distance)); - pitch_rotation = static_cast(180 / PI * atan2((attack_distance*tan(origin_pitch*PI/180)-real_hit_point.y), attack_distance)); - - /*origin_yaw = mark_yaw; - origin_pitch = mark_pitch;*/ - -// if(position_mode == 1){ -// yaw_rotation = static_cast(180 / PI * atan2((attack_distance * tan(origin_yaw * PI / 180) - real_hit_point.x), attack_distance)); -// pitch_rotation = static_cast(180 / PI * atan2((attack_distance*tan(origin_pitch*PI/180)-real_hit_point.y), attack_distance)); -// } -// else if(position_mode == 2){ -// yaw_rotation = static_cast(180 / PI * atan2((attack_distance * tan(origin_yaw * PI / 180) - real_hit_point.x), attack_distance)); -// pitch_rotation = static_cast(180 / PI * atan2((attack_distance*tan(origin_pitch*PI/180)-real_hit_point.y), attack_distance)); -// } -// else if(position_mode == 3){ -// yaw_rotation = static_cast(180 / PI * atan2((attack_distance * tan(origin_yaw * PI / 180) - real_hit_point.x), attack_distance)); -// pitch_rotation = static_cast(180 / PI * atan2((attack_distance*tan(origin_pitch*PI/180)-real_hit_point.y), attack_distance)); -// } -// else if(position_mode == 4){ -// yaw_rotation = static_cast(180 / PI * atan2((attack_distance * tan(origin_yaw * PI / 180) - real_hit_point.x), attack_distance)); -// pitch_rotation = static_cast(180 / PI * atan2((attack_distance*tan(origin_pitch*PI/180)-real_hit_point.y), attack_distance)); -// } -// else if(position_mode == 5){ -// yaw_rotation = static_cast(180 / PI * atan2((attack_distance * tan(origin_yaw * PI / 180) - real_hit_point.x), attack_distance)); -// pitch_rotation = static_cast(180 / PI * atan2((attack_distance*tan(origin_pitch*PI/180)-real_hit_point.y), attack_distance)); -// } -// else if(position_mode == 6){ -// yaw_rotation = static_cast(180 / PI * atan2((attack_distance * tan(origin_yaw * PI / 180) - real_hit_point.x), attack_distance)); -// pitch_rotation = static_cast(180 / PI * atan2((attack_distance*tan(origin_pitch*PI/180)-real_hit_point.y), attack_distance)); -// } -// else{ -// pitch_rotation = 5.5+static_cast(180 / PI * atan2((attack_distance*tan(mark_pitch*PI/180)-real_hit_point.y), attack_distance)); -// } - + cv::Point2f real_predict_point;//计算在真实世界中的预测点位，进而计算云台的旋转角度 + stretch(predict_point, real_predict_point); + yaw_rotation = static_cast(180 / PI * + atan2((attack_distance * tan(origin_yaw * PI / 180) - real_predict_point.x), attack_distance)); + pitch_rotation = static_cast(180 / PI * + atan2((attack_distance*tan(origin_pitch*PI/180)-real_predict_point.y), attack_distance)); } \ No newline at end of file diff --git a/energy/src/energy/get/hit_point_get.cpp b/energy/src/energy/get/hit_point_get.cpp deleted file mode 100644 index 9394d7c..0000000 --- a/energy/src/energy/get/hit_point_get.cpp +++ /dev/null @@ -1,133 +0,0 @@ -// -// Created by xixiliadorabarry on 1/24/19. -// - -#include "energy/energy.h" -#include "energy/constant.h" - -using namespace cv; -using std::cout; -using std::endl; -using std::vector; - -void Energy::getHitPoint(){ - int rad = 60; - if(energy_part_rotation==1) rotate(rad, radius, cycle_center, target_center, hit_point); - if(energy_part_rotation==-1) rotate(-rad, radius, cycle_center, target_center, hit_point); -// int hit_position = 1000; -// int limit_angle = 6; -// int angle_interval = 60; -// -// if(energy_part_rotation==1){ -// if(target_armor>=0 && target_armor<=limit_angle){ -// hit_point.x = cycle_center.x + static_cast(radius / 2); -// hit_point.y = cycle_center.y + static_cast(radius * sqrt(3) / 2); -// hit_position = -1*angle_interval; -// position_mode = 1; -// } -// else if(target_armor>=angle_interval && target_armor(radius); -// hit_point.y = cycle_center.y; -// hit_position = 0; -// position_mode = 2; -// } -// else if(target_armor>=angle_interval*2 && target_armor<=angle_interval*2+limit_angle){ -// hit_point.x = cycle_center.x + static_cast(radius / 2); -// hit_point.y = cycle_center.y - static_cast(radius * sqrt(3) / 2); -// hit_position = angle_interval; -// position_mode = 3; -// } -// else if(target_armor>=-180 && target_armor<-180+limit_angle){ -// hit_point.x = cycle_center.x - static_cast(radius / 2); -// hit_point.y = cycle_center.y - static_cast(radius * sqrt(3) / 2); -// hit_position = 2*angle_interval; -// position_mode = 4; -// } -// else if(target_armor>=-2*angle_interval&&target_armor<-2*angle_interval+limit_angle){ -// hit_point.x = cycle_center.x - static_cast(radius); -// hit_point.y = cycle_center.y; -// hit_position = 180; -// position_mode = 5; -// } -// else if(target_armor>=-1*angle_interval&&target_armor<-1*angle_interval+limit_angle) { -// hit_point.x = cycle_center.x - static_cast(radius / 2); -// hit_point.y = cycle_center.y + static_cast(radius * sqrt(3) / 2); -// hit_position = -2 * angle_interval; -// position_mode = 6; -// } -// else{ -// position_mode = 0; -// return; -// } -// } -// -// if(energy_part_rotation==-1){ -// if(target_armor>=0 && target_armor<=limit_angle){ -// hit_point.x = cycle_center.x - static_cast(radius / 2); -// hit_point.y = cycle_center.y - static_cast(radius * sqrt(3) / 2); -// hit_position = 2*angle_interval; -// position_mode = 1; -// } -// else if(target_armor>=angle_interval && target_armor(radius); -// hit_point.y = cycle_center.y; -// hit_position = 180; -// position_mode = 2; -// } -// else if(target_armor>=angle_interval*2 && target_armor<=angle_interval*2+limit_angle){ -// hit_point.x = cycle_center.x - static_cast(radius / 2); -// hit_point.y = cycle_center.y + static_cast(radius * sqrt(3) / 2); -// hit_position = -2*angle_interval; -// position_mode = 3; -// } -// else if(target_armor>=-180 && target_armor<-180+limit_angle){ -// hit_point.x = cycle_center.x + static_cast(radius / 2); -// hit_point.y = cycle_center.y + static_cast(radius * sqrt(3) / 2); -// hit_position = -1*angle_interval; -// position_mode = 4; -// } -// else if(target_armor>=-2*angle_interval&&target_armor<-2*angle_interval+limit_angle){ -// hit_point.x = cycle_center.x + static_cast(radius); -// hit_point.y = cycle_center.y; -// hit_position = 0; -// position_mode = 5; -// } -// else if(target_armor>=-1*angle_interval&&target_armor<-1*angle_interval+limit_angle){ -// hit_point.x = cycle_center.x + static_cast(radius / 2); -// hit_point.y = cycle_center.y - static_cast(radius * sqrt(3) / 2); -// hit_position = angle_interval; -// position_mode = 6; -// } -// else{ -// position_mode = 0; -// return; -// } -// } -// -// if(position_mode!=0 && position_mode!=last_position_mode){ -// last_position_mode = position_mode; -// isSendTarget = true; -// //cout<<"hit position: "< 330){ - last_target_armor = target_armor; - return false; - } - else{ - last_target_armor = target_armor; - return true; - } -} \ No newline at end of file diff --git a/energy/src/energy/get/position_get.cpp b/energy/src/energy/get/position_get.cpp index 5a409d9..6ca5ddb 100644 --- a/energy/src/energy/get/position_get.cpp +++ b/energy/src/energy/get/position_get.cpp @@ -8,42 +8,50 @@ using std::cout; using std::endl; using std::vector; -void Energy::getFanPosition(std::vector &fanPosition, const std::vector &fans, cv::Point cycle_center, double radius) { + + + +//---------------------------------------------------------------------------------------------------------------------- +// 此函数用于获得图像中所有扇叶的当前极坐标角度 +// --------------------------------------------------------------------------------------------------------------------- +void Energy::getFanPosition() { if (radius == 0)return; for (const auto &fan : fans) { - float angle = static_cast(180 / PI * atan2(-1 * (fan.rect.center.y - cycle_center.y), (fan.rect.center.x - cycle_center.x))); - fanPosition.push_back(angle); + float angle = static_cast(180 / PI * atan2(-1 * (fan.rect.center.y - circle_center_point.y), + (fan.rect.center.x - circle_center_point.x))); + fan_polar_angle.push_back(angle); } // cout << "fanPosition.size() = " << fanPosition.size() << '\t' << endl; } -void Energy::getArmorPosition(std::vector &armorPosition, const std::vector &armors, cv::Point cycle_center, double radius) { + + + +//---------------------------------------------------------------------------------------------------------------------- +// 此函数用于获得图像中所有装甲板的当前极坐标角度 +// --------------------------------------------------------------------------------------------------------------------- +void Energy::getArmorPosition() { if (radius == 0)return; for (const auto &armor : armors) { - float angle = static_cast(180 / PI * atan2(-1 * (armor.rect.center.y - cycle_center.y), (armor.rect.center.x - cycle_center.x))); - armorPosition.push_back(angle); + float angle = static_cast(180 / PI * atan2(-1 * (armor.rect.center.y - circle_center_point.y), + (armor.rect.center.x - circle_center_point.x))); + armor_polar_angle.push_back(angle); } // cout << "armorPosition.size() = " << armorPosition.size() << '\t' << endl; } -void Energy::getFirstArmorCenters(vector &armors, std::vector &first_armor_centers) -{ - for (const auto &armor : armors) { - if (armors.size() < 2)first_armor_centers.push_back(armor.rect.center); - } -} + + +//---------------------------------------------------------------------------------------------------------------------- +// 此函数用于存储图像中所有装甲板的中心坐标，以便后续最小二乘法计算圆心和半径 +// --------------------------------------------------------------------------------------------------------------------- void Energy::getAllArmorCenters() { for (const auto &armor : armors) { all_armor_centers.push_back(armor.rect.center); } -} - -void Energy::getPosition(cv::Point point, double &angle){ - if (radius == 0)return; - angle = (180 / PI * atan2(-1 * (point.y - cycle_center.y), (point.x - cycle_center.x))); -} +} \ No newline at end of file diff --git a/energy/src/energy/param_init.cpp b/energy/src/energy/param_init.cpp index 5ca1d9f..dc48ae4 100644 --- a/energy/src/energy/param_init.cpp +++ b/energy/src/energy/param_init.cpp @@ -10,34 +10,22 @@ using std::endl; using std::vector; void Energy::initEnergy() { - isSendTarget = false; isMark = false; fans_cnt = 0; armors_cnt = 0; centerRs_cnt = 0; - cycle_center = Point(0, 0); - target_center = Point(0, 0); - last_target_center = Point(0, 0); - hit_point = Point(0, 0); - target_position = -1000; - last_target_position = -1000; - last_hit_position = 20000; - target_armor = -1000; - last_target_armor = -1000; + circle_center_point = Point(0, 0); + target_point = Point(0, 0); + predict_point = Point(0, 0); + target_polar_angle = -1000; + last_target_polar_angle = -1000; radius = 0; - - // ally_color = ALLY_RED; - energy_part_rotation = CLOCKWISE; + energy_rotation_direction = CLOCKWISE; attack_distance = ATTACK_DISTANCE; - count = 1; last_fans_cnt = 0; last_armors_cnt = 0; send_cnt = 0; - - //rectified_focal_length = 1000; - //theta = 0; - //phi = 0; yaw_rotation = 0; pitch_rotation = 0; last_mark = 0; @@ -48,30 +36,16 @@ void Energy::initEnergy() { blue_origin_pitch = 15.688477; target_cnt = 0; - target_cnt_flag = true; - - isLeftVertexFound = -1; - isTopVertexFound = -1; - isRightVertexFound = -1; - isBottomVertexFound = -1; - - left = Point(640, 480); - right = Point(0, 0); - top = Point(640, 480); - bottom = Point(0, 0); - - position_mode = 0; - last_position_mode = 0; - energy_rotation_init = false; + predict_rad = 20; fans.clear(); armors.clear(); centerRs.clear(); - fanPosition.clear(); - armorPosition.clear(); - Armor_center.clear(); - first_armor_centers.clear(); + + fan_polar_angle.clear(); + armor_polar_angle.clear(); + all_armor_centers.clear(); clockwise_rotation_init_cnt = 0; @@ -116,37 +90,27 @@ void Energy::initEnergyPartParam() { energy_part_param_.CENTER_R_CONTOUR_HW_RATIO_MIN = 1; energy_part_param_.TWIN_ANGEL_MAX = 10; - - lift_height_.LIFT_0 = 0; - lift_height_.LIFT_30 = 0; - lift_height_.LIFT_60 = 0; - lift_height_.LIFT_90 = 10; - lift_height_.LIFT_minus_30 = 0; - lift_height_.LIFT_minus_60 = 0; - lift_height_.LIFT_minus_90 = 0; - } void Energy::initRotation() { - target_position = target_armor; -// cout << "target position: " << target_position << '\t' << "last target position: " << last_target_position << endl; - if (target_position >= -180 && last_target_position >= -180 && fabs(target_position - last_target_position) < 30) { - if (target_position < last_target_position) clockwise_rotation_init_cnt++; - else if (target_position > last_target_position) anticlockwise_rotation_init_cnt++; + if (target_polar_angle >= -180 && last_target_polar_angle >= -180 + && fabs(target_polar_angle - last_target_polar_angle) < 30) { + //target_polar_angle和last_target_polar_angle的初值均为1000，大于-180表示刚开始几帧不要 + //若两者比较接近，则说明没有切换目标，因此可以用于顺逆时针的判断 + if (target_polar_angle < last_target_polar_angle) clockwise_rotation_init_cnt++; + else if (target_polar_angle > last_target_polar_angle) anticlockwise_rotation_init_cnt++; } - + //由于刚开始圆心判断不准，角度变化可能计算有误，因此需要在角度正向或逆向变化足够大时才可确定是否为顺逆时针 if (clockwise_rotation_init_cnt == 30) { - energy_part_rotation = CLOCKWISE; - cout << "rotation: " << energy_part_rotation << endl; + energy_rotation_direction = CLOCKWISE;//顺时针变化30次，确定为顺时针 + cout << "rotation: " << energy_rotation_direction << endl; energy_rotation_init = false; } else if (anticlockwise_rotation_init_cnt == 30) { - energy_part_rotation = ANTICLOCKWISE; - cout << "rotation: " << energy_part_rotation << endl; + energy_rotation_direction = ANTICLOCKWISE;//逆时针变化30次，确定为顺时针 + cout << "rotation: " << energy_rotation_direction << endl; energy_rotation_init = false; } -// else cout << clockwise_rotation_init_cnt <<'\t'<0)showFanContours("fan",src,fans); +// if(fans_cnt>0)showFanContours("fan",src); // fans_cnt=0; armors_cnt = findArmor(src, last_armors_cnt); // cout<<"armors_cnt: "<0) showArmorContours("armor",src,armors); +// if(armors_cnt>0) showArmorContours("armor",src); if(armors_cnt>0||fans_cnt>0) showBothContours("Both",src); centerRs_cnt = findCenterR(src); if(centerRs_cnt>0)showCenterRContours("R",src); - if(armors_cnt>=4 && fans_cnt>=3) { + if(armors_cnt>=4 && fans_cnt>=3) { FILE *fp = fopen(PROJECT_DIR"/Mark/mark.txt", "w"); if (fp) { fprintf(fp, "yaw: %f, pitch: %f\n", origin_yaw, origin_pitch); @@ -69,75 +65,32 @@ int Energy::run(cv::Mat &src){ fclose(fp_best); } } -// cout<<"armors_cnt: "<0)hit_point = gimble_zero_points.at(0).rect.center; -*/ - diff --git a/energy/src/energy/send_target_by_uart/send_target_by_uart.cpp b/energy/src/energy/send_target_by_uart/send_target_by_uart.cpp index 4dd180a..3de33d8 100644 --- a/energy/src/energy/send_target_by_uart/send_target_by_uart.cpp +++ b/energy/src/energy/send_target_by_uart/send_target_by_uart.cpp @@ -7,22 +7,10 @@ using namespace std; bool sendTarget(Serial& serial, float x, float y, float z) { static short x_tmp, y_tmp, z_tmp; -// static time_t last_time = time(nullptr); -// static int fps; uint8_t buff[8]; - -// time_t t = time(nullptr); -// if (last_time != t) { -// last_time = t; -// std::cout << "fps:" << fps << ", (" << x << "," << y << "," << z << ")" << std::endl; -// fps = 0; -// } -// fps += 1; - x_tmp = static_cast(x * (32768 - 1) / 100); y_tmp = static_cast(y * (32768 - 1) / 100); z_tmp = static_cast(z * (32768 - 1) / 1000); - buff[0] = 's'; buff[1] = static_cast((x_tmp >> 8) & 0xFF); buff[2] = static_cast((x_tmp >> 0) & 0xFF); @@ -31,14 +19,11 @@ bool sendTarget(Serial& serial, float x, float y, float z) { buff[5] = static_cast((z_tmp >> 8) & 0xFF); buff[6] = static_cast((z_tmp >> 0) & 0xFF); buff[7] = 'e'; - return serial.WriteData(buff, sizeof(buff)); } void Energy::sendTargetByUart(float x, float y, float z) { -// if(!changeTarget()){ -// return; -// } + if(changeTarget())target_cnt++; sendTarget(serial, x, y, z); send_cnt+=1; // cout<<"send"< &fans) { +//---------------------------------------------------------------------------------------------------------------------- +// 此函数用于显示图像中所有扇叶 +// --------------------------------------------------------------------------------------------------------------------- +void Energy::showFanContours(std::string windows_name, const cv::Mat src) { if (src.empty())return; static Mat image2show; @@ -33,7 +36,10 @@ void Energy::showFanContours(std::string windows_name, const cv::Mat &src, const imshow(windows_name, image2show); } -void Energy::showArmorContours(std::string windows_name, const cv::Mat &src, const std::vector &armors) { +//---------------------------------------------------------------------------------------------------------------------- +// 此函数用于显示图像中所有装甲板 +// --------------------------------------------------------------------------------------------------------------------- +void Energy::showArmorContours(std::string windows_name, const cv::Mat src) { if (src.empty())return; static Mat image2show; @@ -58,6 +64,47 @@ void Energy::showArmorContours(std::string windows_name, const cv::Mat &src, con imshow(windows_name, image2show); } +void Energy::showBothContours(std::string windows_name, const cv::Mat src) { + if (src.empty())return; + static Mat image2show; + if(src.type() == CV_8UC1) // 黑白图像 + { + cvtColor(src, image2show, COLOR_GRAY2RGB); + + } else if (src.type() == CV_8UC3) //RGB 彩色 + { + image2show = src.clone(); + } + //cvtColor(image2show, image2show, COLOR_GRAY2RGB); + for (const auto &fan : fans) + { + Point2f vertices[4]; //定义矩形的4个顶点 + fan.rect.points(vertices); //计算矩形的4个顶点 + for (int i = 0; i < 4; i++) + line(image2show, vertices[i], vertices[(i + 1) % 4], Scalar(255, 0, 0), 4); +// cout << "fan" << fan.rect.size.height <<'\t'<< fan.rect.size.width << '\t' << '\t'; +// cout << endl; + } + for (const auto &armor : armors) + { + Point2f vertices[4]; //定义矩形的4个顶点 + armor.rect.points(vertices); //计算矩形的4个顶点 + for (int i = 0; i < 4; i++) + line(image2show, vertices[i], vertices[(i + 1) % 4], Scalar(0, 0, 255), 4); +// cout << "armor center: "<< armor.rect.center << '\t'<< "armor angle: "<< armor.rect.angle; +// cout << endl; + + cv::Point2f point = armor.rect.center; + cv::circle(image2show, point, 2, cv::Scalar(0, 0, 255));//在图像中画出特征点，2是圆的半径 + + } + + imshow(windows_name, image2show); +} + +//---------------------------------------------------------------------------------------------------------------------- +// 此函数用于显示图像中所有可能的风车中心候选区R +// --------------------------------------------------------------------------------------------------------------------- void Energy::showCenterRContours(std::string windows_name, const cv::Mat src) { if (src.empty())return; static Mat image2show; @@ -83,42 +130,9 @@ void Energy::showCenterRContours(std::string windows_name, const cv::Mat src) { imshow(windows_name, image2show); } -void Energy::showBothContours(std::string windows_name, const cv::Mat src) { - if (src.empty())return; - static Mat image2show; - if(src.type() == CV_8UC1) // 黑白图像 - { - cvtColor(src, image2show, COLOR_GRAY2RGB); - - } else if (src.type() == CV_8UC3) //RGB 彩色 - { - image2show = src.clone(); - } - //cvtColor(image2show, image2show, COLOR_GRAY2RGB); - for (const auto &fan : fans) - { - Point2f vertices[4]; //定义矩形的4个顶点 - fan.rect.points(vertices); //计算矩形的4个顶点 - for (int i = 0; i < 4; i++) - line(image2show, vertices[i], vertices[(i + 1) % 4], Scalar(255, 0, 0), 4); -// cout << "fan" << fan.rect.size.height <<'\t'<< fan.rect.size.width << '\t' << '\t'; -// cout << endl; - } - for (const auto &armor : armors) - { - Point2f vertices[4]; //定义矩形的4个顶点 - armor.rect.points(vertices); //计算矩形的4个顶点 - for (int i = 0; i < 4; i++) - line(image2show, vertices[i], vertices[(i + 1) % 4], Scalar(0, 0, 255), 4); -// cout << "armor center: "<< armor.rect.center << '\t'<< "armor angle: "<< armor.rect.angle; -// cout << endl; - - cv::Point2f point = armor.rect.center; - cv::circle(image2show, point, 2, cv::Scalar(0, 0, 255));//在图像中画出特征点，2是圆的半径 - - } - - imshow(windows_name, image2show); -} +//---------------------------------------------------------------------------------------------------------------------- +// 此函数用于显示图像中所有扇叶和装甲板 +// --------------------------------------------------------------------------------------------------------------------- + diff --git a/energy/src/energy/tool/tool.cpp b/energy/src/energy/tool/tool.cpp index 24ba1e2..cc6f402 100644 --- a/energy/src/energy/tool/tool.cpp +++ b/energy/src/energy/tool/tool.cpp @@ -20,69 +20,27 @@ void Energy::extract(cv::Mat &src){ imshow("extract", src); } -void Energy::saveFourPoints(std::vector &FourPoints, cv::Point point_1, cv::Point point_2, cv::Point point_3, cv::Point point_4) { - FourPoints.push_back(point_1); - FourPoints.push_back(point_2); - FourPoints.push_back(point_3); - FourPoints.push_back(point_4); -} - -void Energy::savePoint2f(std::vector &point_save, cv::Point point) { - point_save.push_back(static_cast(point)); -} - -double Energy::pointDistance(cv::Point point_1, cv::Point point_2){ - double distance = 0; - distance = sqrt(pow(static_cast(point_1.x - point_2.x),2) - + pow(static_cast(point_1.y - point_2.y),2)); - return distance; -} - -void Energy::rotate(double rad, double radius, cv::Point center, cv::Point point_old, cv::Point &point_new) { +void Energy::rotate() { int x1, x2, y1, y2; // 为了减小强制转换的误差 - x1 = center.x * 100; - x2 = point_old.x * 100; - y1 = center.y * 100; - y2 = point_old.y * 100; + x1 = circle_center_point.x * 100; + x2 = target_point.x * 100; + y1 = circle_center_point.y * 100; + y2 = target_point.y * 100; - point_new.x = static_cast((x1 + (x2 - x1)*cos(-rad * d2r) - (y1 - y2)*sin(-rad * d2r))/100); - point_new.y = static_cast((y1 - (x2 - x1)*sin(-rad * d2r) - (y1 - y2)*cos(-rad * d2r))/100); + predict_point.x = static_cast((x1 + (x2 - x1)*cos(-predict_rad * d2r) - (y1 - y2)*sin(-predict_rad * d2r))/100); + predict_point.y = static_cast((y1 - (x2 - x1)*sin(-predict_rad * d2r) - (y1 - y2)*cos(-predict_rad * d2r))/100); } void Energy::stretch(cv::Point point_1, cv::Point2f &point_2){ - if(point_1==cycle_center){ - cout<<"stretch wrong!"<( ARMOR_CENTER_TO_CYCLE_CENTER * x_0 / r_0); point_2.y = static_cast( ARMOR_CENTER_TO_CYCLE_CENTER * y_0 / r_0); -} - -void Energy::cycle(cv::Point p1, cv::Point p2, cv::Point p3, cv::Point ¢er, double &radius){ - double x1, y1, x2, y2, x3, y3; - double a, b, c, g, e, f; - x1 = p1.x; - y1 = p1.y; - x2 = p2.x; - y2 = p2.y; - x3 = p3.x; - y3 = p3.y; - - //三点确定圆的方程：(2x2-2x1)X+(2y2-2y1)Y=x2²-x1²+y2²-y1²；(2x3-2x2)X+(2y3-2y2)Y=x3²-x2²+y3²-y2² - e = 2 * (x2 - x1); - f = 2 * (y2 - y1); - g = x2 * x2 - x1 * x1 + y2 * y2 - y1 * y1; - a = 2 * (x3 - x2); - b = 2 * (y3 - y2); - c = x3 * x3 - x2 * x2 + y3 * y3 - y2 * y2; - cycle_center.x = static_cast((g*b - c * f) / (e*b - a * f)); - cycle_center.y = static_cast((a*g - c * e) / (a*f - b * e)); - radius = sqrt((cycle_center.x - x1)*(cycle_center.x - x1) + (cycle_center.y - y1)*(cycle_center.y - y1)); - -} - +} \ No newline at end of file diff --git a/main.cpp b/main.cpp index 63392b7..c989982 100644 --- a/main.cpp +++ b/main.cpp @@ -96,8 +96,6 @@ int main(int argc, char *argv[]) { ArmorFinder armorFinder(mcuData.enemy_color, serial, PROJECT_DIR"/tools/para/", mcuData.use_classifier); Energy energy(serial, mcuData.enemy_color); -// energy.setAllyColor(ally_color); - energy.setRotation(CLOCKWISE); bool ok = true; cout<<"start running"<