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

修改，PID发现并未优化

Browse Source
This commit is contained in:
Liu Lida
2026-03-24 09:38:50 +08:00
parent 05a8782eea
commit 2b272a3bd3
2 changed files with 7 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

@@ -57,17 +57,18 @@ 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 += dx;
sum_yaw += yaw;
sum_pitch += dy;
float yaw_I_component = YAW_AIM_KI * sum_yaw;
float pitch_I_component = PITCH_AIM_KI * sum_pitch;
double tmp_yaw = dx;
double tmp_yaw = yaw;
double tmp_pitch = dy;
dx = YAW_AIM_KP * dx + YAW_AIM_KI * sum_yaw +
YAW_AIM_KD * (dx - last_yaw);
yaw = YAW_AIM_KP * yaw + YAW_AIM_KI * sum_yaw +
YAW_AIM_KD * (yaw - last_yaw);
dy = PITCH_AIM_KP * dy + PITCH_AIM_KI * sum_pitch +
PITCH_AIM_KD * (dy - last_pitch);
@@ -75,7 +76,7 @@ bool ArmorFinder::sendBoxPosition(uint16_t shoot_delay) {
last_pitch = tmp_pitch;
//
double yaw = atan(dx / FOCUS_PIXAL) * 180 / PI;
// double yaw = atan(dx / FOCUS_PIXAL) * 180 / PI;
double dist = sqrt(target_xyz.x * target_xyz.x + target_xyz.y * target_xyz.y + target_xyz.z * target_xyz.z) / 100.0; // 米
// 弹道补偿使用 PnP 提供的 3D 坐标

2
others/src/serial.cpp
View File

@@ -181,7 +181,7 @@ bool Serial::ReadData(unsigned char *buffer, unsigned int length) {
using namespace std;
string get_uart_dev_name() {
FILE *ls = popen("ls /dev/ttyCH341USB* --color=never", "r");
FILE *ls = popen("ls /dev/ttyUSB* --color=never", "r");
char name[20] = {0};
fscanf(ls, "%s", name);
return name;