energy changed

energy/include/energy/energy.h

@@ -25,7 +25,10 @@ class Energy {
 public:
 	Energy(Serial &u, uint8_t &color);//构造函数，参数为串口和敌方颜色
 	~Energy();//默认析构函数
-	int run(cv::Mat &gimble_src, cv::Mat &base_src);
+	int runBig(cv::Mat &gimble_src, cv::Mat &chassis_src);
+    int runBig(cv::Mat &gimble_src);
+    int runSmall(cv::Mat &gimble_src, cv::Mat &chassis_src);
+    int runSmall(cv::Mat &gimble_src);
 	Serial &serial;//串口
 	void setEnergyRotationInit();//判断顺逆时针函数
 	void extract(cv::Mat &src);//框取图像中的一块区域进行处理
@@ -99,6 +102,7 @@ private:
 
     void rotate();//获取预测点位
 	void stretch(cv::Point point_1, cv::Point2f &point_2);//将像素差转换为实际距离差
+    double pointDistance(cv::Point point_1, cv::Point point_2);//计算两点距离
 
 	void writeDownMark();//记录操作手标定的云台初始角度
 

energy/src/energy/param_init.cpp

@@ -26,7 +26,7 @@ void Energy::initEnergy() {
     target_polar_angle = -1000;
     last_target_polar_angle = -1000;
 	radius = 0;
-    energy_rotation_direction = CLOCKWISE;
+    energy_rotation_direction = ANTICLOCKWISE;
 	attack_distance = ATTACK_DISTANCE;
 	last_fans_cnt = 0;
 	last_armors_cnt = 0;

energy/src/energy/run.cpp

@@ -12,12 +12,23 @@ using std::vector;
 
 
 //----------------------------------------------------------------------------------------------------------------------
-// 此函数为能量机关模式主控制流函数
+// 此函数为大能量机关模式主控制流函数，且步兵需要同时拥有云台摄像头和底盘摄像头
 // ---------------------------------------------------------------------------------------------------------------------
-int Energy::run(cv::Mat &gimble_src, cv::Mat &base_src){
+int Energy::runBig(cv::Mat &gimble_src, cv::Mat &chassis_src){
+    if(chassis_src.empty())runBig(gimble_src);//仅拥有云台摄像头则调用单摄像头的run函数
+    else {
+        runBig(chassis_src);
+        return 0;
+    }
+}
 
-    cv::Mat src = gimble_src;
 
+
+
+//----------------------------------------------------------------------------------------------------------------------
+// 此函数为大能量机关模式主控制流函数，且步兵仅拥有云台摄像头
+// ---------------------------------------------------------------------------------------------------------------------
+int Energy::runBig(cv::Mat &gimble_src){
 //    imshow("src",src);
     fans.clear();
     armors.clear();
@@ -25,35 +36,29 @@ int Energy::run(cv::Mat &gimble_src, cv::Mat &base_src){
     fan_polar_angle.clear();
     armor_polar_angle.clear();
 
-	changeMark();
-	if (isMark)return 0;
+    changeMark();
+    if (isMark)return 0;
 //    imagePreprocess(src);
 //    imshow("img_preprocess",src);
 
-    threshold(src, src, energy_part_param_.GRAY_THRESH, 255, THRESH_BINARY);
+    threshold(gimble_src, gimble_src, energy_part_param_.GRAY_THRESH, 255, THRESH_BINARY);
 //    imshow("bin",src);
 
-    fans_cnt = findFan(src, last_fans_cnt);
+    fans_cnt = findFan(gimble_src, last_fans_cnt);
 //    cout<<"fans_cnt: "<<fans_cnt<<endl;
     if(fans_cnt==-1) return 0;//滤去漏判的帧
 //    if(fans_cnt>0)showFanContours("fan",src);
 //    fans_cnt=0;
 
-    armors_cnt = findArmor(src, last_armors_cnt);
+    armors_cnt = findArmor(gimble_src, last_armors_cnt);
 //    cout<<"armors_cnt: "<<armors_cnt<<endl;
     if(armors_cnt==-1) return 0;//滤去漏判的帧
 //    if(armors_cnt>0) showArmorContours("armor",src);
 
-    if(armors_cnt>0||fans_cnt>0) showBothContours("Both",src);
+    if(armors_cnt != fans_cnt+1) return 0;
 
-    centerRs_cnt = findCenterR(src);
-    if(centerRs_cnt>0)showCenterRContours("R",src);
-
-
-    if(armors_cnt != fans_cnt+1)
-    {
-        return 0;
-    }
+    centerRs_cnt = findCenterR(gimble_src);
+//    if(centerRs_cnt>0)showCenterRContours("R", gimble_src);
 
     getAllArmorCenters();
     circleLeastFit();
@@ -63,10 +68,12 @@ int Energy::run(cv::Mat &gimble_src, cv::Mat &base_src){
     getArmorPolarAngle();
     findTarget();
 
-	if (energy_rotation_init) {
-		initRotation();
-		return 0;
-	}
+    if(armors_cnt>0||fans_cnt>0) showBothContours("Both", gimble_src);
+
+    if (energy_rotation_init) {
+        initRotation();
+        return 0;
+    }
     getPredictPoint();
     gimbleRotation();
     sendTargetByUart(yaw_rotation, pitch_rotation, target_cnt);
@@ -79,3 +86,26 @@ int Energy::run(cv::Mat &gimble_src, cv::Mat &base_src){
 
 
 
+
+//----------------------------------------------------------------------------------------------------------------------
+// 此函数为小能量机关模式主控制流函数，且步兵需要同时拥有云台摄像头和底盘摄像头
+// ---------------------------------------------------------------------------------------------------------------------
+int Energy::runSmall(cv::Mat &gimble_src, cv::Mat &chassis_src){
+    if(chassis_src.empty())runSmall(gimble_src);//仅拥有云台摄像头则调用单摄像头的run函数
+    else return 0;
+}
+
+
+
+
+
+//----------------------------------------------------------------------------------------------------------------------
+// 此函数为小能量机关模式主控制流函数，且步兵仅拥有云台摄像头
+// ---------------------------------------------------------------------------------------------------------------------
+int Energy::runSmall(cv::Mat &gimble_src){
+
+}
+
+
+
+

energy/src/energy/show/show.cpp

@@ -8,6 +8,8 @@ using std::cout;
 using std::endl;
 using std::vector;
 
+
+
 //----------------------------------------------------------------------------------------------------------------------
 // 此函数用于显示图像中所有扇叶
 // ---------------------------------------------------------------------------------------------------------------------
@@ -23,19 +25,18 @@ void Energy::showFanContours(std::string windows_name, const cv::Mat src) {
     {
         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), 2);
-		//cout << fan.rect.center << '\t' << fan.rect.angle << '\t';
-		//cout << endl;
 	}
 	imshow(windows_name, image2show);
 }
 
+
+
 //----------------------------------------------------------------------------------------------------------------------
 // 此函数用于显示图像中所有装甲板
 // ---------------------------------------------------------------------------------------------------------------------
@@ -51,19 +52,21 @@ void Energy::showArmorContours(std::string windows_name, const cv::Mat src) {
     {
         image2show = src.clone();
     }
-	//cvtColor(image2show, image2show, COLOR_GRAY2RGB);
 	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), 2);
-		//cout << armor.rect.center << '\t' << armor.rect.angle << '\t';
-		//cout << endl;
 	}
 	imshow(windows_name, image2show);
 }
 
+
+
+//----------------------------------------------------------------------------------------------------------------------
+// 此函数用于显示图像中所有扇叶和装甲板,并框出待击打装甲板
+// ---------------------------------------------------------------------------------------------------------------------
 void Energy::showBothContours(std::string windows_name, const cv::Mat src) {
     if (src.empty())return;
     static Mat image2show;
@@ -75,25 +78,23 @@ void Energy::showBothContours(std::string windows_name, const cv::Mat src) {
     {
         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;
-
+        for (int i = 0; i < 4; i++){
+            if(pointDistance(static_cast<cv::Point2f>(armor.rect.center),target_point) < 5)
+                line(image2show, vertices[i], vertices[(i + 1) % 4], Scalar(255, 255, 0), 4);
+            else
+                line(image2show, vertices[i], vertices[(i + 1) % 4], Scalar(0, 0, 255), 4);
+        }
         cv::Point2f point = armor.rect.center;
         cv::circle(image2show, point, 2, cv::Scalar(0, 0, 255));//在图像中画出特征点，2是圆的半径
 
@@ -102,6 +103,8 @@ void Energy::showBothContours(std::string windows_name, const cv::Mat src) {
     imshow(windows_name, image2show);
 }
 
+
+
 //----------------------------------------------------------------------------------------------------------------------
 // 此函数用于显示图像中所有可能的风车中心候选区R
 // ---------------------------------------------------------------------------------------------------------------------

energy/src/energy/tool/tool.cpp

@@ -58,4 +58,16 @@ void Energy::stretch(cv::Point point_1, cv::Point2f &point_2){
 
 	point_2.x = static_cast<float >( ARMOR_CENTER_TO_CYCLE_CENTER * x_0 / r_0);
 	point_2.y = static_cast<float >( ARMOR_CENTER_TO_CYCLE_CENTER * y_0 / r_0);
+}
+
+
+//----------------------------------------------------------------------------------------------------------------------
+// 此函数用于计算两点距离
+// ---------------------------------------------------------------------------------------------------------------------
+
+double Energy::pointDistance(cv::Point point_1, cv::Point point_2){
+    double distance = 0;
+    distance = sqrt(pow(static_cast<double>(point_1.x - point_2.x),2)
+                    + pow(static_cast<double>(point_1.y - point_2.y),2));
+    return distance;
 }