Auto Aim

energy/include/energy/energy.h

@@ -40,6 +40,7 @@ private:
 
 	EnergyPartParam energy_part_param_;
 	LiftHeight lift_height_;
+	bool isSendTarget;
 	int fans_cnt;
 	int armors_cnt;
 	int count;

energy/src/energy/energy.cpp

@@ -12,6 +12,8 @@ Energy::Energy(Uart &u):uart(u),
         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);

energy/src/energy/get/hit_point_get.cpp

@@ -12,75 +12,81 @@ using std::vector;
 
 void Energy::getHitPoint(){
     int hit_position = 1000;
+    int limit_angle = 3;
+    int angle_interval = 60;
 
     if(energy_part_rotation==1){    //顺时针
-        if(target_armor>=0&&target_armor<60){
+        if(target_armor>=0 && target_armor<=limit_angle){
             hit_point.x = cycle_center.x + static_cast<int>(radius / 2);
             hit_point.y = cycle_center.y + static_cast<int>(radius * sqrt(3) / 2);
-            hit_position = -60;
+            hit_position = -1*angle_interval;
         }
-        if(target_armor>=60&&target_armor<120){
+        if(target_armor>=angle_interval && target_armor<angle_interval+limit_angle){
             hit_point.x = cycle_center.x + static_cast<int>(radius);
             hit_point.y = cycle_center.y;
             hit_position = 0;
         }
-        if(target_armor>=120&&target_armor<=180){
+        if(target_armor>=angle_interval*2 && target_armor<=angle_interval*2+limit_angle){
             hit_point.x = cycle_center.x + static_cast<int>(radius / 2);
             hit_point.y = cycle_center.y - static_cast<int>(radius * sqrt(3) / 2);
-            hit_position = 60;
+            hit_position = angle_interval;
         }
-        if(target_armor>=-180&&target_armor<-120){
+        if(target_armor>=-180 && target_armor<-180+limit_angle){
             hit_point.x = cycle_center.x - static_cast<int>(radius / 2);
             hit_point.y = cycle_center.y - static_cast<int>(radius * sqrt(3) / 2);
-            hit_position = 120;
+            hit_position = 2*angle_interval;
         }
-        if(target_armor>=-120&&target_armor<-60){
+        if(target_armor>=-2*angle_interval&&target_armor<-2*angle_interval+limit_angle){
             hit_point.x = cycle_center.x - static_cast<int>(radius);
             hit_point.y = cycle_center.y;
             hit_position = 180;
         }
-        if(target_armor>=-60&&target_armor<0){
+        if(target_armor>=-1*angle_interval&&target_armor<-1*angle_interval+limit_angle) {
             hit_point.x = cycle_center.x - static_cast<int>(radius / 2);
             hit_point.y = cycle_center.y + static_cast<int>(radius * sqrt(3) / 2);
-            hit_position = -120;
+            hit_position = -2 * angle_interval;
         }
+        else return;
     }
 
     if(energy_part_rotation==-1){    //逆时针
-        if(target_armor>=0&&target_armor<60){
+        if(target_armor>=0 && target_armor<=limit_angle){
             hit_point.x = cycle_center.x - static_cast<int>(radius / 2);
             hit_point.y = cycle_center.y - static_cast<int>(radius * sqrt(3) / 2);
-            hit_position = 120;
+            hit_position = 2*angle_interval;
         }
-        if(target_armor>=60&&target_armor<120){
+        if(target_armor>=angle_interval && target_armor<angle_interval+limit_angle){
             hit_point.x = cycle_center.x - static_cast<int>(radius);
             hit_point.y = cycle_center.y;
             hit_position = 180;
         }
-        if(target_armor>=120&&target_armor<=180){
+        if(target_armor>=angle_interval*2 && target_armor<=angle_interval*2+limit_angle){
             hit_point.x = cycle_center.x - static_cast<int>(radius / 2);
             hit_point.y = cycle_center.y + static_cast<int>(radius * sqrt(3) / 2);
-            hit_position = -120;
+            hit_position = -2*angle_interval;
         }
-        if(target_armor>=-180&&target_armor<-120){
+        if(target_armor>=-180 && target_armor<-180+limit_angle){
             hit_point.x = cycle_center.x + static_cast<int>(radius / 2);
             hit_point.y = cycle_center.y + static_cast<int>(radius * sqrt(3) / 2);
-            hit_position = -60;
+            hit_position = -1*angle_interval;
         }
-        if(target_armor>=-120&&target_armor<-60){
+        if(target_armor>=-2*angle_interval&&target_armor<-2*angle_interval+limit_angle){
             hit_point.x = cycle_center.x + static_cast<int>(radius);
             hit_point.y = cycle_center.y;
             hit_position = 0;
         }
-        if(target_armor>=-60&&target_armor<0){
+        if(target_armor>=-1*angle_interval&&target_armor<-1*angle_interval+limit_angle){
             hit_point.x = cycle_center.x + static_cast<int>(radius / 2);
             hit_point.y = cycle_center.y - static_cast<int>(radius * sqrt(3) / 2);
-            hit_position = 60;
+            hit_position = angle_interval;
         }
+        else return;
     }
 
     cout<<"hit position: "<<hit_position<<endl;
 
+    isSendTarget = true;
+
 //    double rad = static_cast<double>(energy_part_rotation * energy_part_param_.RPM
 //                * energy_part_param_.HIT_TIME * 360 / 60);
 //    rotate(rad, radius, cycle_center, target_center, hit_point);

energy/src/energy/run.cpp

@@ -66,6 +66,9 @@ int Energy::run(cv::Mat &src){
     cout << "The hit point position is " << hit_point << endl;
 //    hit_point = cycle_center;
 
+    if(!isSendTarget)return 0;
+
+
     gimbleRotation();
 
     sendTargetByUart(yaw_rotation, pitch_rotation, attack_distance);