energy changed

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

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();//操作手手动修改标定值

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
 

energy/src/energy/calculate/cycle_calculate.cpp

@@ -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"<<endl;
-        return;
-    }
-    double sum_x = 0.0f, sum_y = 0.0f;
-    double sum_x2 = 0.0f, sum_y2 = 0.0f;
-    double sum_x3 = 0.0f, sum_y3 = 0.0f;
-    double sum_xy = 0.0f, sum_x1y2 = 0.0f, sum_x2y1 = 0.0f;
-    int N = static_cast<int>(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<int>(a / (-2));
-    circle_center_point.y = static_cast<int>(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;
-}

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)
     {

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));

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);

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);//无法被匹配到的装甲板为待击打装甲板

energy/src/energy/get/gimble_rotation_get.cpp

@@ -39,7 +39,7 @@ void Energy::changeMark() {
 
 
 //----------------------------------------------------------------------------------------------------------------------
-// 此函数用于计算云台应当转到角度
+// 此函数用于计算云台应当转到的角度
 // ---------------------------------------------------------------------------------------------------------------------
 void Energy::gimbleRotation(){
 	cv::Point2f real_predict_point;//计算在真实世界中的预测点位，进而计算云台的旋转角度

energy/src/energy/get/position_get.cpp

@@ -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<float>(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<float>(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);
-	}
-}

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);
+        }
+    }
+}

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) {

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) {

energy/src/energy/send_target_by_uart/send_target_by_uart.cpp

@@ -5,6 +5,11 @@
 #include <iostream>
 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);

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<int>((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!"<<endl;