diff --git a/armor/src/armor_finder/send_target/send_target.cpp b/armor/src/armor_finder/send_target/send_target.cpp index f0b8e6f..30de334 100644 --- a/armor/src/armor_finder/send_target/send_target.cpp +++ b/armor/src/armor_finder/send_target/send_target.cpp @@ -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 坐标 diff --git a/others/src/serial.cpp b/others/src/serial.cpp index d2b599f..6abfc75 100644 --- a/others/src/serial.cpp +++ b/others/src/serial.cpp @@ -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;