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);