From 7ba9b7633515bae2344a5bc30015a70da21a57ab Mon Sep 17 00:00:00 2001 From: Li Da <3199335945@qq.com> Date: Tue, 24 Mar 2026 13:08:31 +0800 Subject: [PATCH] =?UTF-8?q?=E4=BC=98=E5=8C=96=E5=8F=91=E5=8C=85?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- armor/src/armor_finder/send_target/send_target.cpp | 11 +++++------ 1 file changed, 5 insertions(+), 6 deletions(-) diff --git a/armor/src/armor_finder/send_target/send_target.cpp b/armor/src/armor_finder/send_target/send_target.cpp index 30de334..ee13b93 100644 --- a/armor/src/armor_finder/send_target/send_target.cpp +++ b/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);