优化发包

armor/src/armor_finder/send_target/send_target.cpp

@@ -41,7 +41,7 @@ static bool sendTarget(Serial &serial, double yaw, uint16_t shoot_delay, bool fi
 
     // Vofa串口验证 不用可以注释掉
     char buff[128];
-    int len = sprintf(buff,"channels: %f, %f, %d\n", yaw, shoot_delay, fire);
+    int len = sprintf(buff,"channels: %f, %u, %d\n", yaw, (unsigned int)shoot_delay, fire);
     return serial.WriteData((unsigned char *)buff, len);
 }
 
@@ -57,18 +57,17 @@ bool ArmorFinder::sendBoxPosition(uint16_t shoot_delay) {
     auto rect = target_box.rect;
     double dx = rect.x + rect.width / 2 - IMAGE_CENTER_X;
     double dy = rect.y + rect.height / 2 - IMAGE_CENTER_Y;
-    double yaw = atan(dx / FOCUS_PIXAL) * 180 / PI;
 
     // PID
-    sum_yaw += yaw;
+    sum_yaw += dx;
     sum_pitch += dy;
     float yaw_I_component = YAW_AIM_KI * sum_yaw;
     float pitch_I_component = PITCH_AIM_KI * sum_pitch;
 
-    double tmp_yaw = yaw;
+    double tmp_yaw = dx;
     double tmp_pitch = dy;
-    yaw = YAW_AIM_KP * yaw + YAW_AIM_KI * sum_yaw +
-                   YAW_AIM_KD * (yaw - last_yaw);
+    dx = YAW_AIM_KP * dx + YAW_AIM_KI * sum_yaw +
+                   YAW_AIM_KD * (dx - last_yaw);
     dy = PITCH_AIM_KP * dy + PITCH_AIM_KI * sum_pitch +
                      PITCH_AIM_KD * (dy - last_pitch);