VSAG/vision_hero
3
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

优化发包

Browse Source
This commit is contained in:
Liu Lida
2026-03-24 13:08:31 +08:00
parent 2b272a3bd3
commit 7ba9b76335
1 changed files with 5 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files

11
armor/src/armor_finder/send_target/send_target.cpp
View File

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