大符：云台摄像头

.gitignore

@@ -4,3 +4,4 @@ build
 Mark
 tools/TrainCNN/.idea
 tools/TrainCNN/__pycache__
+.DS_Store

energy/include/energy/energy.h

@@ -18,7 +18,6 @@
 #include "additions/additions.h"
 #include "options/options.h"
 
-
 using std::vector;
 
 class Energy {
@@ -38,11 +37,13 @@ public:
 private:
 	EnergyPartParam energy_part_param_;//能量机关的参数设置
 	bool isMark;//若操作手正在手动标定，则为true
+	bool centered=false;//云台是否对准中心
 	int fans_cnt;//图像中的扇叶个数
 	int armors_cnt;//图像中的装甲板个数
 	int centerRs_cnt;//图像中可能的风车中心字母R选区个数
 	int last_fans_cnt;//上一帧的扇叶个数
 	int last_armors_cnt;//上一帧的装甲板个数
+	int gimble_cnt; //经过的帧数
 	double radius;//大风车半径
 	float target_polar_angle;//待击打装甲板的极坐标角度
     float last_target_polar_angle;//上一帧待击打装甲板的极坐标角度
@@ -62,7 +63,6 @@ private:
 	float origin_yaw, origin_pitch;//初始的云台角度设定值
 	float target_cnt;//用于记录寻找到的装甲板总数，该值变化则立即中断主控板发射进程，防止重复击打已点亮的装甲板
     bool save_new_mark;//若操作手进行过手动标定，则为true
-
 	std::vector<EnergyPart> fans;//图像中所有扇叶
 	std::vector<EnergyPart> armors;//图像中所有装甲板
     std::vector<EnergyPart> centerRs;//风车中心字母R的可能候选区
@@ -70,6 +70,7 @@ private:
 	cv::Point circle_center_point;//风车圆心坐标
 	cv::Point target_point;//目标装甲板中心坐标
 	cv::Point predict_point;//预测的击打点坐标
+	cv::Point former_point;//之前预测的圆心坐标
 	std::vector<float>fan_polar_angle;//当前帧所有扇叶的极坐标角度
 	std::vector<float>armor_polar_angle;//当前帧所有装甲板的极坐标角度
     std::vector<cv::Point> all_armor_centers;//记录全部的装甲板中心，用于风车圆心和半径的计算

energy/src/energy/param_init.cpp

@@ -16,13 +16,15 @@ using std::vector;
 // ---------------------------------------------------------------------------------------------------------------------
 void Energy::initEnergy() {
 	isMark = false;
-
+	centered=false;
 	fans_cnt = 0;
 	armors_cnt = 0;
 	centerRs_cnt = 0;
+	gimble_cnt = 0;
     circle_center_point = Point(0, 0);
 	target_point = Point(0, 0);
     predict_point = Point(0, 0);
+	former_point = Point(0,0);
     target_polar_angle = -1000;
     last_target_polar_angle = -1000;
 	radius = 0;

energy/src/energy/run.cpp

@@ -15,11 +15,53 @@ using std::vector;
 // 此函数为大能量机关模式主控制流函数，且步兵需要同时拥有云台摄像头和底盘摄像头
 // ---------------------------------------------------------------------------------------------------------------------
 int Energy::runBig(cv::Mat &gimble_src, cv::Mat &chassis_src){
-    if(chassis_src.empty())runBig(gimble_src);//仅拥有云台摄像头则调用单摄像头的run函数
-    else {
-        runBig(chassis_src);
+    if(chassis_src.empty())
+        runBig(gimble_src);//仅拥有云台摄像头则调用单摄像头的run函数
+    else if(!centered) {
+        armors.clear();
+        armor_polar_angle.clear();
+        changeMark();
+        if (isMark)return 0;
+
+        threshold(gimble_src, gimble_src, energy_part_param_.GRAY_THRESH, 255, THRESH_BINARY);
+        imshow("yun",gimble_src);
+
+        armors_cnt = findArmor(gimble_src,  last_armors_cnt);
+        if(armors_cnt!=1) return 0;//滤去漏判的帧
+
+        getAllArmorCenters();
+        circleLeastFit();
+
+//    attack_distance = 752;//单项赛
+        attack_distance = 718;
+
+        if (energy_rotation_init) {
+            initRotation();
             return 0;
         }
+
+        if(++gimble_cnt==10){
+            former_point=circle_center_point;
+            gimble_cnt=0;
+        }
+
+        if(former_point==predict_point&&gimble_cnt==9&&predict_point!=Point(0,0)) {
+            centered=true;
+            cout<<"gimble focused!"<<endl;
+        }
+        predict_point=circle_center_point;
+        cout<<gimble_cnt<<endl;
+        cout<<"center:("<<predict_point.x<<','<<predict_point.y<<")\n";
+        gimbleRotation();
+
+//    cout<<"send"<<endl;
+//    cout<<"position mode: "<<position_mode<<endl;
+        sendTargetByUart(yaw_rotation, pitch_rotation, target_cnt);
+        return 0;
+    }
+//    if(centered)
+//        destroyAllWindows();
+    return 0;
 }
 
 
@@ -81,6 +123,7 @@ int Energy::runBig(cv::Mat &gimble_src){
 //    cout<<"yaw: "<<yaw_rotation<<'\t'<<"pitch: "<<pitch_rotation<<endl;
 //    cout<<"curr_yaw: "<<mcuData.curr_yaw<<'\t'<<"curr_pitch: "<<mcuData.curr_pitch<<endl;
 //    cout<<"send_cnt: "<<send_cnt<<endl;
+    return 0;
 }