energy update

energy/include/energy/energy.h

@@ -31,6 +31,7 @@ public:
 
 //    void setAllyColor(int color);
     void setRotation(int rotation);
+	void setEnergyRotationInit();
 
 	void extract(cv::Mat &src);
 
@@ -42,6 +43,7 @@ private:
 	EnergyPartParam energy_part_param_;
 	LiftHeight lift_height_;
 	bool isSendTarget;
+	bool isMark;
 	int fans_cnt;
 	int armors_cnt;
 	int count;
@@ -61,13 +63,18 @@ private:
 	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 origin_yaw, origin_pitch;
 
 	std::vector<EnergyPart> fans;
 	std::vector<EnergyPart> armors;
-    std::vector<EnergyPart> gimble_zero_points;
+ //   std::vector<EnergyPart> gimble_zero_points;
 
 	cv::Point cycle_center;
 	cv::Point target_center;
@@ -81,9 +88,9 @@ private:
     cv::Point left, right, top, bottom;
 	cv::Mat src_blue, src_red, src_green;
     
-
-
+	void initEnergy();
 	void initEnergyPartParam();
+	void initRotation();
 
 	int findFan(const cv::Mat &src, vector<EnergyPart> &fans, int &last_fans_cnt);
 	int findArmor(const cv::Mat &src, vector<EnergyPart> &armors, int &last_armors_cnt);
@@ -121,6 +128,7 @@ private:
 
     void getHitPoint();
 	bool changeTarget();
+	void changeMark();
     void gimbleRotation();
 
 	void splitBayerBG(cv::Mat &src, cv::Mat &blue, cv::Mat &red);

energy/src/energy/energy.cpp

@@ -8,51 +8,13 @@ using std::cout;
 using std::endl;
 using std::vector;
 
+extern uint8_t last_state;
+
 Energy::Energy(Serial &u, uint8_t &enemy_color):serial(u),ally_color(enemy_color),
         src_blue(SRC_HEIGHT, SRC_WIDTH, CV_8UC1),
         src_red(SRC_HEIGHT, SRC_WIDTH, CV_8UC1)
 {
-    isSendTarget = false;
-
-	fans_cnt = 0;
-	armors_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 = -1;
-	last_target_position = -1;
-    last_hit_position = 20000;
-	target_armor = -1;
-	radius = 0;
-
-//    ally_color = ALLY_RED;
-    energy_part_rotation = 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;
-
-	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;
-
+	initEnergy();
 	initEnergyPartParam();
 }
 
@@ -68,3 +30,8 @@ void Energy::setRotation(int rotation){
 	energy_part_rotation = rotation;
 }
 
+void Energy::setEnergyRotationInit() {
+	initEnergy();
+	initEnergyPartParam();
+	energy_rotation_init = true;
+}

energy/src/energy/get/gimble_rotation_get.cpp

@@ -9,16 +9,28 @@ using std::cout;
 using std::endl;
 using std::vector;
 
-extern float curr_yaw, curr_pitch, mark_yaw, mark_pitch;
+void Energy::changeMark() {
+	if (mcuData.mark == 0 && last_mark == 1) {
+		last_mark = mcuData.mark;
+		origin_yaw = mcuData.curr_yaw;
+		origin_pitch = mcuData.curr_pitch;
+		isMark = true;
+	}
+	else if (mcuData.state == 1) {
+		last_mark = mcuData.mark;
+		isMark = true;
+	}
+	else {
+		last_mark = mcuData.mark;
+		isMark = false;
+	}
+}
 
 
 void Energy::gimbleRotation(){
 	cv::Point2f real_hit_point;
     stretch(hit_point, real_hit_point);
 
-	float origin_yaw = -0.13; 
-	float origin_pitch = 13.18;
-
 	/*origin_yaw = mark_yaw;
 	origin_pitch = mark_pitch;*/
    

energy/src/energy/get/hit_point_get.cpp

@@ -118,13 +118,13 @@ void Energy::getHitPoint(){
     }
 }
 
-bool Energy::changeTarget(){
-    if(fabs(target_position - last_target_position) < 30||fabs(target_position - last_target_position) > 330){
-        last_target_position = target_position;
-        return false;
-    }
-    else{
-        last_target_position = target_position;
-        return true;
-    }
-}
+//bool Energy::changeTarget(){
+//    if(fabs(target_position - last_target_position) < 30||fabs(target_position - last_target_position) > 330){
+//        last_target_position = target_position;
+//        return false;
+//    }
+//    else{
+//        last_target_position = target_position;
+//        return true;
+//    }
+//}

energy/src/energy/param_init.cpp

@@ -8,6 +8,66 @@ using std::cout;
 using std::endl;
 using std::vector;
 
+void Energy::initEnergy() {
+	isSendTarget = false;
+	isMark = false;
+
+	fans_cnt = 0;
+	armors_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;
+	radius = 0;
+
+	//    ally_color = ALLY_RED;
+	energy_part_rotation = 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;
+	origin_yaw = -0.13;
+	origin_pitch = 13.18;
+
+	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;
+
+	fans.clear();
+	armors.clear();
+	fanPosition.clear();
+	armorPosition.clear();
+	Armor_center.clear();
+	first_armor_centers.clear();
+	all_armor_centers.clear();
+
+	clockwise_rotation_init_cnt = 0;
+	anticlockwise_rotation_init_cnt = 0;
+}
+
 void Energy::initEnergyPartParam() {
 
     energy_part_param_.RPM = 10;
@@ -49,3 +109,25 @@ void Energy::initEnergyPartParam() {
 }
 
 
+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 (clockwise_rotation_init_cnt == 5) {
+		energy_part_rotation = CLOCKWISE;
+		cout << "rotation: " << energy_part_rotation << endl;
+		energy_rotation_init = false;
+	}
+	else if (anticlockwise_rotation_init_cnt == 5) {
+		energy_part_rotation = ANTICLOCKWISE;
+		cout << "rotation: " << energy_part_rotation << endl;
+		energy_rotation_init = false;
+	}
+	//else cout << clockwise_rotation_init_cnt << endl;
+
+	last_target_position = target_position;
+}

energy/src/energy/run.cpp

@@ -18,10 +18,11 @@ int Energy::run(cv::Mat &src){
     armors.clear();
     fanPosition.clear();
     armorPosition.clear();
-    gimble_zero_points.clear();
+//    gimble_zero_points.clear();
     isSendTarget = false;
 
-    //if(mark==0)return 0;
+	changeMark();
+	if (isMark)return 0;
 
 //    if(all_armor_centers.size()>200)all_armor_centers.clear();
 //    if(first_armor_centers.size()>200)first_armor_centers.clear();
@@ -57,7 +58,7 @@ int Energy::run(cv::Mat &src){
 //    radius = 116.936;
 //    attack_distance = ATTACK_DISTANCE * 120/ radius;
 
-	attack_distance = 794 + 1245 * 75 * (1/radius - 1/113.9);
+	attack_distance = 794 + 1245 * 75 * (1/radius - 1/115.6);
 //	cout << "attack distance: " << attack_distance << endl;
 
     getFanPosition(fanPosition, fans, cycle_center, radius);
@@ -66,21 +67,24 @@ int Energy::run(cv::Mat &src){
 //    cout << "The target armor's position is " << target_armor << endl;
 //    cout<<"The target armor center is: "<<target_center<<endl;
 
+	if (energy_rotation_init) {
+		initRotation();
+		return 0;
+	}
+
     getHitPoint();
 //    hit_point = target_center;
 //    cout << "The hit point position is " << hit_point << endl;
-
-
-    if(!isSendTarget)return 0;
+    
 //    cout<<"send"<<endl;
     cout<<"position mode: "<<position_mode<<endl;
 
     gimbleRotation();
+	if (!isSendTarget)return 0;
 
     sendTargetByUart(yaw_rotation, pitch_rotation, attack_distance);
     cout<<"yaw: "<<yaw_rotation<<'\t'<<"pitch: "<<pitch_rotation<<endl;
     cout<<"curr_yaw: "<<mcuData.curr_yaw<<'\t'<<"curr_pitch: "<<mcuData.curr_pitch<<endl;
-    cout<<"mark_yaw: "<<mcuData.mark_yaw<<'\t'<<"mark_pitch: "<<mcuData.mark_pitch<<endl;
 
 //    cout<<"send_cnt: "<<send_cnt<<endl;