From da45e62806edc65b66a9322edfd43dc66786aaa8 Mon Sep 17 00:00:00 2001 From: sun Date: Fri, 5 Jul 2019 15:53:12 +0800 Subject: [PATCH] energy changed --- energy/include/energy/constant.h | 2 +- energy/include/energy/energy.h | 8 ++- energy/include/energy/param_struct_define.h | 9 --- .../src/energy/calculate/cycle_calculate.cpp | 63 ------------------- energy/src/energy/calibrate/split.cpp | 11 ++++ energy/src/energy/calibrate/structing.cpp | 9 +++ energy/src/energy/energy.cpp | 18 +++++- energy/src/energy/find/target_finder.cpp | 7 ++- energy/src/energy/get/gimble_rotation_get.cpp | 2 +- energy/src/energy/get/position_get.cpp | 57 ----------------- energy/src/energy/mark/mark.cpp | 31 +++++++++ energy/src/energy/param_init.cpp | 19 ++++-- energy/src/energy/run.cpp | 34 +++------- .../send_target_by_uart.cpp | 10 +++ energy/src/energy/tool/tool.cpp | 15 +++++ 15 files changed, 127 insertions(+), 168 deletions(-) delete mode 100644 energy/src/energy/calculate/cycle_calculate.cpp delete mode 100644 energy/src/energy/get/position_get.cpp create mode 100644 energy/src/energy/mark/mark.cpp diff --git a/energy/include/energy/constant.h b/energy/include/energy/constant.h index 802c421..ea661e9 100644 --- a/energy/include/energy/constant.h +++ b/energy/include/energy/constant.h @@ -17,7 +17,7 @@ const int SRC_HEIGHT = 240; const double PI = 3.1415926; const int CLOCKWISE = 1; const int ANTICLOCKWISE = -1; -const float ATTACK_DISTANCE = 770;//cm +const float ATTACK_DISTANCE = 718;//cm const double WHOLE_FAN = 80;//cm const double ARMOR_CENTER_TO_CYCLE_CENTER = 75;//cm //const double ARMOR_CENTER_TO_CYCLE_CENTER = 71;//cm diff --git a/energy/include/energy/energy.h b/energy/include/energy/energy.h index 9aac222..a358a2d 100644 --- a/energy/include/energy/energy.h +++ b/energy/include/energy/energy.h @@ -89,17 +89,19 @@ private: void showBothContours(std::string windows_name, const cv::Mat src);//显示扇叶和装甲板 void showCenterRContours(std::string windows_name, const cv::Mat src);//显示风车中心候选区R - void getFanPosition();//获取扇叶极坐标角度 - void getArmorPosition();//获取装甲板极坐标角度 + void getFanPolarAngle();//获取扇叶极坐标角度 + void getArmorPolarAngle();//获取装甲板极坐标角度 void getAllArmorCenters();//记录所有装甲板中心坐标 - void cycleLeastFit();//利用所有记录的装甲板中心最小二乘法计算圆心和半径 + void circleLeastFit();//利用所有记录的装甲板中心最小二乘法计算圆心和半径 void findTarget();//获取目标装甲板的极坐标角度和装甲板中心坐标 void rotate();//获取预测点位 void stretch(cv::Point point_1, cv::Point2f &point_2);//将像素差转换为实际距离差 + void writeDownMark();//记录操作手标定的云台初始角度 + void getPredictPoint();//获取预测点位 bool changeTarget();//判断目标是否改变 void changeMark();//操作手手动修改标定值 diff --git a/energy/include/energy/param_struct_define.h b/energy/include/energy/param_struct_define.h index 8b05dc1..587f05c 100644 --- a/energy/include/energy/param_struct_define.h +++ b/energy/include/energy/param_struct_define.h @@ -22,9 +22,6 @@ struct EnergyPart { }; struct EnergyPartParam { - double RPM; - double HIT_TIME; - int GRAY_THRESH; int SPLIT_GRAY_THRESH; int FAN_GRAY_THRESH; @@ -60,12 +57,6 @@ struct EnergyPartParam { float TWIN_ANGEL_MAX; }; -typedef struct GMAngle_t{ - float yaw; - float pitch; -}GMAngle_t; - -extern GMAngle_t aim; #endif //PARAM_STRUCT_DEFINE_H diff --git a/energy/src/energy/calculate/cycle_calculate.cpp b/energy/src/energy/calculate/cycle_calculate.cpp deleted file mode 100644 index 5b75bed..0000000 --- a/energy/src/energy/calculate/cycle_calculate.cpp +++ /dev/null @@ -1,63 +0,0 @@ -// -// Created by xixiliadorabarry on 1/24/19. -// -#include "energy/energy.h" - -using namespace cv; -using std::cout; -using std::endl; -using std::vector; - - - - -//---------------------------------------------------------------------------------------------------------------------- -// 此函数通过最小二乘法计算大风车圆心和半径 -// --------------------------------------------------------------------------------------------------------------------- -void Energy::cycleLeastFit() -{ - 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++) - { - double x = all_armor_centers.at(i).x; - double y = all_armor_centers.at(i).y; - double x2 = x * x; - double y2 = y * y; - sum_x += x; - sum_y += y; - sum_x2 += x2; - sum_y2 += y2; - sum_x3 += x2 * x; - sum_y3 += y2 * y; - sum_xy += x * y; - 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; - G = N * sum_y2 - sum_y * sum_y; - H = N * sum_x2y1 + N * sum_y3 - (sum_x2 + sum_y2) * sum_y; - 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; - 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 c4ab981..3c6dc4b 100644 --- a/energy/src/energy/calibrate/split.cpp +++ b/energy/src/energy/calibrate/split.cpp @@ -8,6 +8,11 @@ using std::cout; using std::endl; using std::vector; + + +//---------------------------------------------------------------------------------------------------------------------- +// 此函数用于分离拜耳阵列 +// --------------------------------------------------------------------------------------------------------------------- void Energy::splitBayerBG(cv::Mat src, cv::Mat &blue, cv::Mat &red) { uchar* data; uchar* bayer_data[2]; @@ -24,6 +29,12 @@ void Energy::splitBayerBG(cv::Mat src, cv::Mat &blue, cv::Mat &red) { } } } + + + +//---------------------------------------------------------------------------------------------------------------------- +// 此函数对图像进行通道分离处理 +// --------------------------------------------------------------------------------------------------------------------- void Energy::imagePreprocess(cv::Mat &src) { if(src.type() == CV_8UC1) { diff --git a/energy/src/energy/calibrate/structing.cpp b/energy/src/energy/calibrate/structing.cpp index 97b2721..5b80be8 100644 --- a/energy/src/energy/calibrate/structing.cpp +++ b/energy/src/energy/calibrate/structing.cpp @@ -8,6 +8,10 @@ using std::cout; using std::endl; using std::vector; + +//---------------------------------------------------------------------------------------------------------------------- +// 此函数对图像进行闭运算操作 +// --------------------------------------------------------------------------------------------------------------------- void Energy::StructingElementClose(cv::Mat &src,int length, int width){ if (src.empty())return; //threshold(src, src, energy_part_param_.CAMERA_GRAY_THRESH, 255, THRESH_BINARY); @@ -15,6 +19,11 @@ void Energy::StructingElementClose(cv::Mat &src,int length, int width){ morphologyEx(src, src, MORPH_CLOSE, element); } + + +//---------------------------------------------------------------------------------------------------------------------- +// 此函数对图像进行腐蚀与膨胀操作 +// --------------------------------------------------------------------------------------------------------------------- void Energy::StructingElementErodeDilate(cv::Mat &src) { cv::Mat src_out, src_out_out; Mat element_dilate_1 = getStructuringElement(MORPH_RECT, Size(3, 3)); diff --git a/energy/src/energy/energy.cpp b/energy/src/energy/energy.cpp index a25d52a..a311ec8 100644 --- a/energy/src/energy/energy.cpp +++ b/energy/src/energy/energy.cpp @@ -9,8 +9,10 @@ using std::cout; using std::endl; using std::vector; -extern uint8_t last_state; +//---------------------------------------------------------------------------------------------------------------------- +// 此函数为能量机关构造函数，只要程序不重启就不会重新构造 +// --------------------------------------------------------------------------------------------------------------------- Energy::Energy(Serial &u, uint8_t &color):serial(u),ally_color(color), src_blue(SRC_HEIGHT, SRC_WIDTH, CV_8UC1), src_red(SRC_HEIGHT, SRC_WIDTH, CV_8UC1) @@ -18,7 +20,7 @@ Energy::Energy(Serial &u, uint8_t &color):serial(u),ally_color(color), initEnergy(); initEnergyPartParam(); - save_new_mark = true; + save_new_mark = false; if(ally_color == ALLY_RED){ origin_yaw = red_origin_yaw; @@ -33,14 +35,24 @@ Energy::Energy(Serial &u, uint8_t &color):serial(u),ally_color(color), } } + + +//---------------------------------------------------------------------------------------------------------------------- +// 此函数为能量机关析构函数，设置为默认 +// --------------------------------------------------------------------------------------------------------------------- Energy::~Energy() = default; + + +//---------------------------------------------------------------------------------------------------------------------- +// 此函数为能量机关再初始化函数，如果未重启程序但重新进入能量机关，则会进行初始化，但不会将save_new_mark置为false +// --------------------------------------------------------------------------------------------------------------------- void Energy::setEnergyRotationInit() { initEnergy(); initEnergyPartParam(); energy_rotation_init = true; - if(!save_new_mark){ + if(save_new_mark){ FILE *fp = fopen(PROJECT_DIR"/Mark/mark.txt", "r"); if(fp){ fscanf(fp,"%f %f",&origin_yaw,&origin_pitch); diff --git a/energy/src/energy/find/target_finder.cpp b/energy/src/energy/find/target_finder.cpp index 0cbef27..d2dce40 100644 --- a/energy/src/energy/find/target_finder.cpp +++ b/energy/src/energy/find/target_finder.cpp @@ -8,6 +8,11 @@ using std::cout; using std::endl; using std::vector; + + +//---------------------------------------------------------------------------------------------------------------------- +// 此函数用于匹配扇叶和装甲板，找到目标装甲板,计算其极坐标角度和中心坐标 +// --------------------------------------------------------------------------------------------------------------------- void Energy::findTarget() { if (fan_polar_angle.size() >= armor_polar_angle.size()) return;//扇叶多于装甲板，识别错误 if (armor_polar_angle.empty())return;//找不到扇叶，识别错误 @@ -26,7 +31,7 @@ void Energy::findTarget() { 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;//若某个扇叶的极坐标角度与第j个装甲板的极坐标角度接近，则两者匹配成功 + continue;//若第i个扇叶的极坐标角度与第j个装甲板的极坐标角度接近，则两者匹配成功，i与j都加1 } else { target_polar_angle = armor_polar_angle.at(j);//无法被匹配到的装甲板为待击打装甲板 diff --git a/energy/src/energy/get/gimble_rotation_get.cpp b/energy/src/energy/get/gimble_rotation_get.cpp index c957e8c..d300c30 100644 --- a/energy/src/energy/get/gimble_rotation_get.cpp +++ b/energy/src/energy/get/gimble_rotation_get.cpp @@ -39,7 +39,7 @@ void Energy::changeMark() { //---------------------------------------------------------------------------------------------------------------------- -// 此函数用于计算云台应当转到角度 +// 此函数用于计算云台应当转到的角度 // --------------------------------------------------------------------------------------------------------------------- void Energy::gimbleRotation(){ cv::Point2f real_predict_point;//计算在真实世界中的预测点位，进而计算云台的旋转角度 diff --git a/energy/src/energy/get/position_get.cpp b/energy/src/energy/get/position_get.cpp deleted file mode 100644 index 6ca5ddb..0000000 --- a/energy/src/energy/get/position_get.cpp +++ /dev/null @@ -1,57 +0,0 @@ -// -// Created by xixiliadorabarry on 1/24/19. -// -#include "energy/energy.h" - -using namespace cv; -using std::cout; -using std::endl; -using std::vector; - - - - -//---------------------------------------------------------------------------------------------------------------------- -// 此函数用于获得图像中所有扇叶的当前极坐标角度 -// --------------------------------------------------------------------------------------------------------------------- -void Energy::getFanPosition() { - if (radius == 0)return; - for (const auto &fan : fans) - { - 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() { - if (radius == 0)return; - for (const auto &armor : armors) - { - 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::getAllArmorCenters() -{ - for (const auto &armor : armors) { - all_armor_centers.push_back(armor.rect.center); - } -} \ No newline at end of file diff --git a/energy/src/energy/mark/mark.cpp b/energy/src/energy/mark/mark.cpp new file mode 100644 index 0000000..9280858 --- /dev/null +++ b/energy/src/energy/mark/mark.cpp @@ -0,0 +1,31 @@ +// +// Created by sun on 19-7-5. +// + +#include "energy/energy.h" + +using namespace std; +using namespace cv; + +void Energy::writeDownMark() { + 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); + fclose(fp); + save_new_mark = true; + } + FILE *fp_all = fopen(PROJECT_DIR"/Mark/mark_all.txt", "a"); + if (fp_all) { + fprintf(fp_all, "yaw: %f, pitch: %f\n", origin_yaw, origin_pitch); + fclose(fp_all); + } + } + if(armors_cnt==5){ + FILE *fp_best = fopen(PROJECT_DIR"/Mark/mark_best.txt", "a"); + if(fp_best){ + fprintf(fp_best, "yaw: %f, pitch: %f\n",origin_yaw, origin_pitch); + fclose(fp_best); + } + } +} \ No newline at end of file diff --git a/energy/src/energy/param_init.cpp b/energy/src/energy/param_init.cpp index dc48ae4..44f2b8b 100644 --- a/energy/src/energy/param_init.cpp +++ b/energy/src/energy/param_init.cpp @@ -9,6 +9,11 @@ using std::cout; using std::endl; using std::vector; + + +//---------------------------------------------------------------------------------------------------------------------- +// 此函数对能量机关成员变量进行初始化 +// --------------------------------------------------------------------------------------------------------------------- void Energy::initEnergy() { isMark = false; @@ -52,11 +57,12 @@ void Energy::initEnergy() { anticlockwise_rotation_init_cnt = 0; } + + +//---------------------------------------------------------------------------------------------------------------------- +// 此函数对能量机关参数进行初始化 +// --------------------------------------------------------------------------------------------------------------------- void Energy::initEnergyPartParam() { - - energy_part_param_.RPM = 10; - energy_part_param_.HIT_TIME = 1.14; - energy_part_param_.GRAY_THRESH = 240; energy_part_param_.SPLIT_GRAY_THRESH = 60; energy_part_param_.FAN_GRAY_THRESH = 75; @@ -93,6 +99,11 @@ void Energy::initEnergyPartParam() { } + + +//---------------------------------------------------------------------------------------------------------------------- +// 此函数对能量机关旋转方向进行初始化 +// --------------------------------------------------------------------------------------------------------------------- void Energy::initRotation() { if (target_polar_angle >= -180 && last_target_polar_angle >= -180 && fabs(target_polar_angle - last_target_polar_angle) < 30) { diff --git a/energy/src/energy/run.cpp b/energy/src/energy/run.cpp index a1ce396..915ab0e 100644 --- a/energy/src/energy/run.cpp +++ b/energy/src/energy/run.cpp @@ -9,10 +9,11 @@ using std::cout; using std::endl; using std::vector; -//extern float curr_yaw, curr_pitch, mark_yaw, mark_pitch; -//extern int mark; +//---------------------------------------------------------------------------------------------------------------------- +// 此函数为能量机关模式主控制流函数 +// --------------------------------------------------------------------------------------------------------------------- int Energy::run(cv::Mat &src){ // imshow("src",src); fans.clear(); @@ -45,37 +46,18 @@ int Energy::run(cv::Mat &src){ centerRs_cnt = findCenterR(src); if(centerRs_cnt>0)showCenterRContours("R",src); - 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); - fclose(fp); - save_new_mark = false; - } - FILE *fp_all = fopen(PROJECT_DIR"/Mark/mark_all.txt", "a"); - if (fp_all) { - fprintf(fp_all, "yaw: %f, pitch: %f\n", origin_yaw, origin_pitch); - fclose(fp_all); - } - } - if(armors_cnt==5){ - FILE *fp_best = fopen(PROJECT_DIR"/Mark/mark_best.txt", "a"); - if(fp_best){ - fprintf(fp_best, "yaw: %f, pitch: %f\n",origin_yaw, origin_pitch); - fclose(fp_best); - } - } + if(armors_cnt != fans_cnt+1) { return 0; } getAllArmorCenters(); - cycleLeastFit(); - attack_distance = 718; + circleLeastFit(); + attack_distance = ATTACK_DISTANCE; - getFanPosition(); - getArmorPosition(); + getFanPolarAngle(); + getArmorPolarAngle(); findTarget(); if (energy_rotation_init) { 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 3de33d8..eccf26c 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 @@ -5,6 +5,11 @@ #include using namespace std; + + +//---------------------------------------------------------------------------------------------------------------------- +// 此函数用于向主控板发送数据 +// --------------------------------------------------------------------------------------------------------------------- bool sendTarget(Serial& serial, float x, float y, float z) { static short x_tmp, y_tmp, z_tmp; uint8_t buff[8]; @@ -22,6 +27,11 @@ bool sendTarget(Serial& serial, float x, float y, float z) { return serial.WriteData(buff, sizeof(buff)); } + + +//---------------------------------------------------------------------------------------------------------------------- +// 此函数用于操作手数据发送 +// --------------------------------------------------------------------------------------------------------------------- void Energy::sendTargetByUart(float x, float y, float z) { if(changeTarget())target_cnt++; sendTarget(serial, x, y, z); diff --git a/energy/src/energy/tool/tool.cpp b/energy/src/energy/tool/tool.cpp index cc6f402..e4a9c7d 100644 --- a/energy/src/energy/tool/tool.cpp +++ b/energy/src/energy/tool/tool.cpp @@ -13,6 +13,11 @@ using std::cout; using std::endl; using std::vector; + + +//---------------------------------------------------------------------------------------------------------------------- +// 此函数选取图像中的一部分进行处理 +// --------------------------------------------------------------------------------------------------------------------- void Energy::extract(cv::Mat &src){ cv::Rect rect(EXTRACT_POINT_X, EXTRACT_POINT_Y, EXTRACT_WIDTH, EXTRACT_HEIGHT); src = src(rect).clone(); @@ -20,6 +25,11 @@ void Energy::extract(cv::Mat &src){ imshow("extract", src); } + + +//---------------------------------------------------------------------------------------------------------------------- +// 此函数用于计算预测的击打点坐标 +// --------------------------------------------------------------------------------------------------------------------- void Energy::rotate() { int x1, x2, y1, y2; // 为了减小强制转换的误差 @@ -32,6 +42,11 @@ void Energy::rotate() { 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==circle_center_point){ // cout<<"stretch wrong!"<