energy change

energy/src/energy/change/target_change.cpp

@@ -7,16 +7,15 @@ using namespace std;
 using namespace cv;
 
 
-
-
 //----------------------------------------------------------------------------------------------------------------------
 // 此函数用于判断目标是否切换
 // ---------------------------------------------------------------------------------------------------------------------
-void Energy::changeTarget(){
+void Energy::changeTarget() {
     if (pointDistance(target_point, last_target_point) < energy_part_param_.TARGET_CHANGE_DISTANCE_MAX) {
         change_target = false;
     } else {
         change_target = true;
     }
     last_target_point = target_point;
+//    cout << change_target << endl;
 }

energy/src/energy/clear/energy_init.cpp

@@ -141,6 +141,9 @@ void Energy::initEnergyPartParam() {
     gimbal_energy_part_param_.TWIN_ANGEL_MAX = 10;
     gimbal_energy_part_param_.TARGET_INTERSETION_CONTOUR_AREA_MIN = 40;
 
+    gimbal_energy_part_param_.TARGET_CHANGE_DISTANCE_MAX = 20;
+    gimbal_energy_part_param_.TWIN_POINT_MAX = 20;
+
 
 
     chassis_energy_part_param_.GRAY_THRESH = 120;//home

energy/src/energy/get/aim_point_get.cpp

@@ -20,6 +20,6 @@ void Energy::getAimPoint(cv::Point target_point) {
     yaw_rotation = atan(dx / FOCUS_PIXAL) * 180 / PI;
     pitch_rotation = atan(dy / FOCUS_PIXAL) * 180 / PI;
 //    cout << "yaw: " << yaw_rotation << '\t' << "pitch: " << pitch_rotation << endl;
-//    cout << "mcuData.delta_x: " << mcuData.delta_x << '\t' << "mcuData.delta_y: " << mcuData.delta_y << endl;
+    cout << "mcuData.delta_x: " << mcuData.delta_x << '\t' << "mcuData.delta_y: " << mcuData.delta_y << endl;
 
 }

energy/src/energy/judge/judge_shoot.cpp

@@ -16,10 +16,10 @@ using namespace cv;
 void Energy::judgeShootInWorld() {
     if (abs(yaw_rotation - mcuData.curr_yaw) < 0.5 && abs(pitch_rotation - mcuData.curr_pitch) < 0.5) {
         shoot = 4;
-        is_predicting = false;
+//        is_predicting = false;
-        is_guessing = true;
+//        is_guessing = true;
-        start_guess = true;
+//        start_guess = true;
-        gettimeofday(&time_start_guess, NULL);
+//        gettimeofday(&time_start_guess, NULL);
         LOGM(STR_CTR(WORD_LIGHT_RED, "Start Guessing!"));
     } else
         shoot = 2;
@@ -30,15 +30,15 @@ void Energy::judgeShootInWorld() {
 // 此函数用于判断云台坐标系下是否可以发弹
 // ---------------------------------------------------------------------------------------------------------------------
 void Energy::judgeShootInGimbal() {
-    if (abs(yaw_rotation) < 0.5 && fabs(pitch_rotation) < 0.5) {
+    if (abs(yaw_rotation) < 0.5 && abs(pitch_rotation) < 0.5) {
-        shoot = 3;
+        shoot = 4;
-        is_predicting = false;
+//        is_predicting = false;
-        is_guessing = true;
+//        is_guessing = true;
-        start_guess = true;
+//        start_guess = true;
-        gettimeofday(&time_start_guess, NULL);
+//        gettimeofday(&time_start_guess, NULL);
 //        LOGM(STR_CTR(WORD_LIGHT_RED, "Start Guessing!"));
     } else
-        shoot = 1;
+        shoot = 2;
 
 //    cout << "yaw: " << yaw_rotation << '\t' << "pitch: " << pitch_rotation << endl;
 }

energy/src/energy/send/send.cpp

@@ -19,20 +19,20 @@ void Energy::sendEnergy() {
             sum_pitch += pitch_rotation;
             yaw_rotation = AIM_KP * yaw_rotation + AIM_KI * sum_yaw;
             pitch_rotation = AIM_KP * pitch_rotation + AIM_KI * sum_pitch;
-        } else if (is_chassis){
+        } else if (is_chassis) {
             sum_yaw += yaw_rotation - mcuData.curr_yaw;
             sum_pitch += pitch_rotation - mcuData.curr_pitch;
-            yaw_rotation = AIM_KP * yaw_rotation + AIM_KI * sum_yaw;
+            yaw_rotation = AIM_KP * (yaw_rotation - mcuData.curr_yaw) + AIM_KI * sum_yaw;
-            pitch_rotation = AIM_KP * pitch_rotation + AIM_KI * sum_pitch;
+            pitch_rotation = AIM_KP * (pitch_rotation - mcuData.curr_pitch) + AIM_KI * sum_pitch;
         }
     }
 
     if (change_target) {
         sendTarget(serial, yaw_rotation, pitch_rotation, 5);
-    } else if (is_predicting) {
+    } else if (is_guessing) {
-        sendTarget(serial, yaw_rotation, pitch_rotation, shoot);
-    } else {
         sendTarget(serial, yaw_rotation, pitch_rotation, 6);
+    } else {
+        sendTarget(serial, yaw_rotation, pitch_rotation, shoot);
     }
 
 }
@@ -51,7 +51,7 @@ void Energy::sendTarget(Serial &serial, float x, float y, float z) {
     time_t t = time(nullptr);
     if (last_time != t) {
         last_time = t;
-        cout << "fps:" << fps << ", (" << x << "," << y << "," << z << ")" << endl;
+//        cout << "fps:" << fps << ", (" << x << "," << y << "," << z << ")" << endl;
         fps = 0;
     }
     fps += 1;