diff --git a/armor/src/armor_finder/armor_finder.cpp b/armor/src/armor_finder/armor_finder.cpp index 96fd655..c2b3332 100644 --- a/armor/src/armor_finder/armor_finder.cpp +++ b/armor/src/armor_finder/armor_finder.cpp @@ -92,11 +92,15 @@ void ArmorFinder::run(cv::Mat &src) { end: if(is_anti_top) { // 判断当前是否为反陀螺模式 antiTop(); - }else if(target_box.rect != cv::Rect2d()) { + } else if(target_box.rect != cv::Rect2d()) { + // 有目标时正常发送 anti_top_cnt = 0; time_seq.clear(); angle_seq.clear(); sendBoxPosition(0); + } else { + // 无目标时也调用 sendBoxPosition，进入无目标状态处理 + sendBoxPosition(0); } if(target_box.rect != cv::Rect2d()){ diff --git a/armor/src/armor_finder/send_target/send_target.cpp b/armor/src/armor_finder/send_target/send_target.cpp index 8529165..cb0ca68 100644 --- a/armor/src/armor_finder/send_target/send_target.cpp +++ b/armor/src/armor_finder/send_target/send_target.cpp @@ -5,135 +5,240 @@ #include #include #include +#include +#include +#include +#include // amadeus_26 协议: 15字节帧 -// | SOF(0xBB 0x77) | x_move(2B) | y_move(2B) | yaw(2B) | pitch(2B) | feed(2B) | key(1B) | crc8(1B) | EOF(0xEE) | +// | 0xBB | 0x77 | x_move(2B) | y_move(2B) | yaw(2B) | pitch(2B) | feed(2B) | switch(1B) | CRC8 | 0xEE | constexpr uint8_t FRAME_HEADER_1 = 0xBB; constexpr uint8_t FRAME_HEADER_2 = 0x77; constexpr uint8_t FRAME_TAIL = 0xEE; constexpr int FRAME_LENGTH = 15; +constexpr int SEND_INTERVAL_MS = 20; // 50Hz -static uint8_t calculateCRC8(const uint8_t *data, size_t len) { - uint8_t crc = 0xFF; - for (size_t i = 0; i < len; i++) { - crc ^= data[i]; - for (int j = 0; j < 8; j++) { - if (crc & 0x80) { - crc = (crc << 1) ^ 0x31; - } else { - crc <<= 1; - } +// 状态定义 +enum TargetState { + HAVE_TARGET, // 有目标 + LOST_TARGET_WAIT, // 丢失目标，等待1秒 + SEARCHING // 索敌状态（持续旋转直到找到目标） +}; + +// 共享控制数据 +struct ControlData { + int16_t yaw = 0; + int16_t pitch = 0; + int16_t feed = 0; + uint8_t left_switch = 3; + uint8_t right_switch = 3; +}; + +static std::mutex control_mutex; +static ControlData shared_control; +static std::atomic sender_running{false}; +static std::thread sender_thread; + +// 发送线程函数 +void senderLoop(Serial &serial) { + uint8_t frame[FRAME_LENGTH]; + + while (sender_running) { + auto start = std::chrono::steady_clock::now(); + + // 获取控制数据（加锁） + ControlData local_control; + { + std::lock_guard lock(control_mutex); + local_control = shared_control; + } + + // 构建数据帧 + int idx = 0; + frame[idx++] = FRAME_HEADER_1; + frame[idx++] = FRAME_HEADER_2; + + // x_move, y_move (固定为0) + frame[idx++] = 0; + frame[idx++] = 0; + frame[idx++] = 0; + frame[idx++] = 0; + + // yaw (小端序) + frame[idx++] = local_control.yaw & 0xFF; + frame[idx++] = (local_control.yaw >> 8) & 0xFF; + + // pitch (小端序) + frame[idx++] = local_control.pitch & 0xFF; + frame[idx++] = (local_control.pitch >> 8) & 0xFF; + + // feed (小端序) + frame[idx++] = local_control.feed & 0xFF; + frame[idx++] = (local_control.feed >> 8) & 0xFF; + + // switch + frame[idx++] = (local_control.left_switch << 4) | local_control.right_switch; + + // CRC8 + frame[idx++] = 0xCC; + + // 帧尾 + frame[idx++] = FRAME_TAIL; + + // 发送数据 + serial.WriteData(frame, FRAME_LENGTH); + + // 精确控制发送频率 + auto elapsed = std::chrono::duration_cast( + std::chrono::steady_clock::now() - start).count(); + int sleep_ms = SEND_INTERVAL_MS - elapsed; + if (sleep_ms > 0) { + std::this_thread::sleep_for(std::chrono::milliseconds(sleep_ms)); } } - return crc; } -static bool sendTarget(Serial &serial, double yaw, double pitch, double dist, uint16_t shoot_delay) { - int16_t x_move = 0; // 平动左右 [-660, 660] - int16_t y_move = 0; // 平动前后 [-660, 660] - int16_t yaw_val = 0; // 云台偏航 [-660, 660] - int16_t pitch_val = 0; // 云台俯仰 [-660, 660] - int16_t feed = 0; // 拨弹轮 [-660, 660] - uint8_t key = 0; // 按键 [0, 15] - -#ifdef WITH_COUNT_FPS - static time_t last_time = time(nullptr); - static int fps; - time_t t = time(nullptr); - if (last_time != t) { - last_time = t; - cout << "Armor: fps:" << fps << ", yaw:" << yaw << ", pitch:" << pitch << ", dist:" << dist << endl; - fps = 0; +// 启动发送线程 +void startSender(Serial &serial) { + if (!sender_running) { + sender_running = true; + sender_thread = std::thread(senderLoop, std::ref(serial)); + LOGM(STR_CTR(WORD_GREEN, "Sender thread started (50Hz)")); } - fps += 1; -#endif +} -#define MINMAX(value, min, max) value = ((value) < (min)) ? (min) : ((value) > (max) ? (max) : (value)) - - // 将角度转换为协议范围 [-660, 660] - yaw_val = static_cast(yaw * 10); // 角度放大10倍 - pitch_val = static_cast(pitch * 10); - MINMAX(yaw_val, -660, 660); - MINMAX(pitch_val, -660, 660); - - // 如果需要发弹，设置 feed 值 - if (shoot_delay > 0) { - feed = 660; // 开火 - } - - // 打印发送的杆量值 - LOGM(STR_CTR(WORD_BLUE, "Armor Send: x_move=%d, y_move=%d, yaw=%d, pitch=%d, feed=%d, key=%d"), - x_move, y_move, yaw_val, pitch_val, feed, key); - - uint8_t frame[FRAME_LENGTH]; - int idx = 0; - - // 帧头 - frame[idx++] = FRAME_HEADER_1; - frame[idx++] = FRAME_HEADER_2; - - // 平动左右 (2 bytes, int16, 小端序) - frame[idx++] = x_move & 0xFF; - frame[idx++] = (x_move >> 8) & 0xFF; - - // 平动前后 (2 bytes, 小端序) - frame[idx++] = y_move & 0xFF; - frame[idx++] = (y_move >> 8) & 0xFF; - - // 云台偏航 (2 bytes, 小端序) - frame[idx++] = yaw_val & 0xFF; - frame[idx++] = (yaw_val >> 8) & 0xFF; - - // 云台俯仰 (2 bytes, 小端序) - frame[idx++] = pitch_val & 0xFF; - frame[idx++] = (pitch_val >> 8) & 0xFF; - - // 拨弹轮 (2 bytes, 小端序) - frame[idx++] = feed & 0xFF; - frame[idx++] = (feed >> 8) & 0xFF; - - // 按键 (1 byte) - frame[idx++] = key; - - // CRC8 (数据区: frame[2]到frame[12], 共11字节) - // 当前固定为0xCC，如需启用CRC校验，取消下面注释 - // frame[idx++] = calculateCRC8(frame + 2, 11); - frame[idx++] = 0xCC; - - // 帧尾 - frame[idx++] = FRAME_TAIL; - - return serial.WriteData(frame, sizeof(frame)); +// 更新控制数据 +void updateControl(int16_t yaw, int16_t pitch, int16_t feed, + uint8_t left_sw = 3, uint8_t right_sw = 3) { + std::lock_guard lock(control_mutex); + shared_control.yaw = yaw; + shared_control.pitch = pitch; + shared_control.feed = feed; + shared_control.left_switch = left_sw; + shared_control.right_switch = right_sw; } bool ArmorFinder::sendBoxPosition(uint16_t shoot_delay) { - if (target_box.rect == cv::Rect2d()) return false; - if (shoot_delay) { - LOGM(STR_CTR(WORD_BLUE, "next box %dms"), shoot_delay); + // 确保发送线程已启动 + static bool sender_started = false; + if (!sender_started) { + startSender(serial); + sender_started = true; } - 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; + + // 状态管理 + static TargetState state = HAVE_TARGET; + static auto state_start_time = std::chrono::steady_clock::now(); + static int16_t pitch_scan = -100; + static int pitch_direction = 1; + + // 检查当前是否有目标 + bool has_target = (target_box.rect != cv::Rect2d()); + auto now = std::chrono::steady_clock::now(); + + // 状态机处理 + switch (state) { + case HAVE_TARGET: + if (!has_target) { + // 目标丢失，进入等待状态 + state = LOST_TARGET_WAIT; + state_start_time = now; + LOGM(STR_CTR(WORD_YELLOW, "Target lost, waiting 1s before searching...")); + } + break; + + case LOST_TARGET_WAIT: + if (has_target) { + // 目标恢复 + state = HAVE_TARGET; + LOGM(STR_CTR(WORD_GREEN, "Target reacquired")); + } else { + // 检查是否等待满1秒 + auto elapsed = std::chrono::duration_cast(now - state_start_time).count(); + if (elapsed >= 1000) { // 1秒到，进入索敌状态 + state = SEARCHING; + LOGM(STR_CTR(WORD_LIGHT_CYAN, "Entering search mode (continuous)")); + } + } + break; + + case SEARCHING: + if (has_target) { + // 目标恢复，退出索敌 + state = HAVE_TARGET; + LOGM(STR_CTR(WORD_GREEN, "Target found, exit search mode")); + } + // 否则持续索敌，不退出 + break; + } + + // 根据状态更新控制值 + if (state == HAVE_TARGET) { + // 有目标时的正常处理 + if (shoot_delay) { + LOGM(STR_CTR(WORD_BLUE, "next box %dms"), 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; - // PID - sum_yaw += dx; - sum_pitch += dy; - float yaw_I_component = YAW_AIM_KI * sum_yaw; - float pitch_I_component = PITCH_AIM_KI * sum_pitch; + // PID + sum_yaw += dx; + sum_pitch += dy; + double tmp_yaw = dx; + double tmp_pitch = dy; + 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); - double tmp_yaw = dx; - double tmp_pitch = dy; - 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); + last_yaw = tmp_yaw; + last_pitch = tmp_pitch; - last_yaw = tmp_yaw; - last_pitch = tmp_pitch; - // - - double yaw = atan(dx / FOCUS_PIXAL) * 180 / PI; - double pitch = atan(dy / FOCUS_PIXAL) * 180 / PI; - double dist = DISTANCE_HEIGHT / rect.height; - return sendTarget(serial, yaw, -pitch, dist, shoot_delay); -} + double yaw = atan(dx / FOCUS_PIXAL) * 180 / PI; + double pitch = atan(dy / FOCUS_PIXAL) * 180 / PI; + + // 转换为杆量值（注意：pitch 方向修正） + int16_t yaw_val = static_cast(yaw * 100); + int16_t pitch_val = static_cast(-pitch * 100); // 取反修正方向 + int16_t feed_val = shoot_delay > 0 ? 660 : 0; + + // 更新控制数据 + updateControl(yaw_val, pitch_val, feed_val, 3, 3); + return true; + + } else if (state == LOST_TARGET_WAIT) { + // 等待1秒期间：开启小陀螺旋转索敌 + static int wait_count = 0; + wait_count++; + if (wait_count % 50 == 0) { + auto remaining = 1000 - std::chrono::duration_cast(now - state_start_time).count(); + LOGM(STR_CTR(WORD_YELLOW, "Pre-search: yaw=150, %dms left"), remaining > 0 ? remaining : 0); + wait_count = 0; + } + // 等待期间也旋转：yaw=150，pitch 小范围扫描 + pitch_scan += pitch_direction * 5; + if (pitch_scan >= 50) pitch_direction = -1; + if (pitch_scan <= -50) pitch_direction = 1; + + updateControl(150, pitch_scan, 0, 3, 2); + return true; + + } else { // SEARCHING + // 索敌状态：持续旋转直到找到目标 + pitch_scan += pitch_direction * 7; + if (pitch_scan >= 200) pitch_direction = -1; + if (pitch_scan <= -100) pitch_direction = 1; + + static int search_count = 0; + search_count++; + if (search_count % 50 == 0) { + LOGM(STR_CTR(WORD_LIGHT_CYAN, "Searching: yaw=150, pitch=%d (waiting for target)"), pitch_scan); + search_count = 0; + } + + // 索敌状态：yaw=150旋转，pitch大范围扫描，小陀螺开启 + updateControl(150, pitch_scan, 0, 3, 2); + return true; + } +} \ No newline at end of file diff --git a/energy/src/energy/send/send.cpp b/energy/src/energy/send/send.cpp index 2c28fd9..dee5668 100644 --- a/energy/src/energy/send/send.cpp +++ b/energy/src/energy/send/send.cpp @@ -60,10 +60,13 @@ void Energy::sendEnergy() { // 此函数用于发送数据给主控板 (amadeus_26 协议) // --------------------------------------------------------------------------------------------------------------------- void Energy::sendTarget(Serial &serial, float yaw, float pitch, int16_t feed, uint8_t key) { - int16_t x_move = 0; // 平动左右 [-660, 660] - int16_t y_move = 0; // 平动前后 [-660, 660] - int16_t yaw_val = 0; // 云台偏航 [-660, 660] - int16_t pitch_val = 0; // 云台俯仰 [-660, 660] + // 控制参数 + int16_t x_move = 0; // 平动左右 [-660, 660] + int16_t y_move = 0; // 平动前后 [-660, 660] + int16_t yaw_val = 0; // 云台偏航 [-660, 660] + int16_t pitch_val = 0; // 云台俯仰 [-660, 660] + uint8_t left_switch = 3; // 左拨杆 [1, 3] 2 摩擦轮打开 3 关闭 + uint8_t right_switch = 3; // 右拨杆 [1, 3] 2 小陀螺 3 关闭 #ifdef WITH_COUNT_FPS static auto last_time = time(nullptr); @@ -78,16 +81,14 @@ void Energy::sendTarget(Serial &serial, float yaw, float pitch, int16_t feed, ui fps += 1; #endif - // 将角度转换为协议范围 [-660, 660] - yaw_val = static_cast(yaw * 10); // 角度放大10倍 - pitch_val = static_cast(pitch * 10); + // 将角度 (-3, 3) 度 转换为协议范围 [-660, 660] + // 映射公式: output = input * (660 / 3) = input * 220 + yaw_val = static_cast(yaw * 220); + pitch_val = static_cast(pitch * 220); MINMAX(yaw_val, -660, 660); MINMAX(pitch_val, -660, 660); - // 打印发送的杆量值 - LOGM(STR_CTR(WORD_LIGHT_PURPLE, "Energy Send: x_move=%d, y_move=%d, yaw=%d, pitch=%d, feed=%d, key=%d"), - x_move, y_move, yaw_val, pitch_val, feed, key); - + // 构建数据帧 - 与 test_pitch_up.cpp 格式一致 uint8_t frame[FRAME_LENGTH]; int idx = 0; @@ -115,17 +116,26 @@ void Energy::sendTarget(Serial &serial, float yaw, float pitch, int16_t feed, ui frame[idx++] = feed & 0xFF; frame[idx++] = (feed >> 8) & 0xFF; - // 按键 (1 byte) - 用于能量机关模式标识 - frame[idx++] = key; + // 拨杆 (1 byte: 高4位左拨杆，低4位右拨杆) + // key 参数通过低4位传递能量机关模式信息 + frame[idx++] = (left_switch << 4) | (key & 0x0F); - // CRC8 (数据区: frame[2]到frame[12], 共11字节) - // 当前固定为0xCC + // CRC8 (固定为 0xCC) frame[idx++] = 0xCC; // 帧尾 frame[idx++] = FRAME_TAIL; + // 调试输出：打印完整帧内容 - 与 test_pitch_up.cpp 格式一致 + std::string frame_hex; + char buf[4]; + for (int i = 0; i < FRAME_LENGTH; i++) { + snprintf(buf, sizeof(buf), "%02X ", frame[i]); + frame_hex += buf; + } + LOGM(STR_CTR(WORD_LIGHT_PURPLE, "[TX](%dB): %s | yaw=%d pitch=%d feed=%d key=%d"), + FRAME_LENGTH, frame_hex.c_str(), yaw_val, pitch_val, feed, key); + serial.WriteData(frame, sizeof(frame)); send_cnt += 1; -// LOGM(STR_CTR(WORD_LIGHT_PURPLE, "send")); } diff --git a/test_pitch_up b/test_pitch_up new file mode 100644 index 0000000..e69de29 diff --git a/test_pitch_up.cpp b/test_pitch_up.cpp new file mode 100644 index 0000000..012dc9e --- /dev/null +++ b/test_pitch_up.cpp @@ -0,0 +1,251 @@ +/** + * @file test_pitch_up.cpp + * @brief 测试程序：单独控制 pitch 轴运动（支持命令行参数） + * + * 用法: ./test_pitch_up [pitch角度] [运行秒数] + * 示例: ./test_pitch_up 1.5 10 (向上1.5度，运行10秒) + * + * 协议帧格式（15字节）： + * | 0xBB | 0x77 | x_move(2B) | y_move(2B) | yaw(2B) | pitch(2B) | feed(2B) | switch(1B) | CRC8 | 0xEE | + */ + +#include +#include +#include +#include +#include +#include +#include +#include + +// 帧定义 - 与示例程序完全一致 +constexpr uint8_t FRAME_HEADER_1 = 0xBB; +constexpr uint8_t FRAME_HEADER_2 = 0x77; +constexpr uint8_t FRAME_TAIL = 0xEE; +constexpr int FRAME_LENGTH = 15; + +// 串口配置 - 与示例程序一致 +constexpr const char *SERIAL_PORT = "/dev/ttyCH340"; +constexpr int BAUDRATE = 115200; + +// CRC8 计算 - 与示例程序完全一致 +uint8_t calculateCRC8(const uint8_t *data, size_t len) { + uint8_t crc = 0xFF; + for (size_t i = 0; i < len; i++) { + crc ^= data[i]; + for (int j = 0; j < 8; j++) { + if (crc & 0x80) { + crc = (crc << 1) ^ 0x31; + } else { + crc <<= 1; + } + } + } + return crc; +} + +// 波特率转换 - 与示例程序一致 +speed_t convertBaudrate(int baudrate) { + switch (baudrate) { + case 9600: return B9600; + case 19200: return B19200; + case 38400: return B38400; + case 57600: return B57600; + case 115200: return B115200; + case 230400: return B230400; + case 460800: return B460800; + case 921600: return B921600; + default: return B115200; + } +} + +int initSerial(const char *port, int baudrate) { + std::cout << "正在打开串口 " << port << "..." << std::endl; + + int fd = open(port, O_RDWR | O_NOCTTY | O_NDELAY); + if (fd < 0) { + std::cerr << "无法打开串口 " << port << ": " << strerror(errno) << std::endl; + return -1; + } + + struct termios tty; + memset(&tty, 0, sizeof(tty)); + + if (tcgetattr(fd, &tty) != 0) { + std::cerr << "tcgetattr 错误: " << strerror(errno) << std::endl; + close(fd); + return -1; + } + + // 设置波特率 + speed_t baud = convertBaudrate(baudrate); + cfsetospeed(&tty, baud); + cfsetispeed(&tty, baud); + + // 8N1 配置 - 与示例程序一致 + tty.c_cflag &= ~PARENB; // 无校验 + tty.c_cflag &= ~CSTOPB; // 1位停止位 + tty.c_cflag &= ~CSIZE; + tty.c_cflag |= CS8; // 8位数据 + tty.c_cflag |= CREAD | CLOCAL; // 启用接收，忽略控制线 + tty.c_cflag &= ~CRTSCTS; // 禁用硬件流控 + + // 原始模式 + tty.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG); + tty.c_iflag &= ~(IXON | IXOFF | IXANY); + tty.c_oflag &= ~OPOST; + + // 设置超时 - 与示例程序一致 + tty.c_cc[VMIN] = 0; + tty.c_cc[VTIME] = 1; // 100ms 超时 + + if (tcsetattr(fd, TCSANOW, &tty) != 0) { + std::cerr << "tcsetattr 错误: " << strerror(errno) << std::endl; + close(fd); + return -1; + } + + // 清空缓冲区 + tcflush(fd, TCIOFLUSH); + std::cout << "串口打开成功" << std::endl; + return fd; +} + +void sendControlFrame(int fd, int16_t pitch_val) { + // 控制参数 + int16_t x_move = -100; // 平动左右 [-660, 660] + int16_t y_move = -200; // 平动前后 [-660, 660] + int16_t yaw_val = -300; // 云台偏航 [-660, 660] + int16_t feed = -400; // 拨弹轮 [-660, 660] + uint8_t left_switch = 3; // 左拨杆 [1, 3] 2 摩擦轮打开 3 关闭 + uint8_t right_switch = 3; // 右拨杆 [1, 3] 2 小陀螺 3 关闭 + + // 构建数据帧 - 与示例程序完全一致 + uint8_t frame[FRAME_LENGTH]; + int idx = 0; + + // 帧头 + frame[idx++] = FRAME_HEADER_1; + frame[idx++] = FRAME_HEADER_2; + + // 平动左右 (2 bytes, int16, 小端序) + frame[idx++] = x_move & 0xFF; + frame[idx++] = (x_move >> 8) & 0xFF; + + // 平动前后 (2 bytes, 小端序) + frame[idx++] = y_move & 0xFF; + frame[idx++] = (y_move >> 8) & 0xFF; + + // 云台偏航 (2 bytes, 小端序) + frame[idx++] = yaw_val & 0xFF; + frame[idx++] = (yaw_val >> 8) & 0xFF; + + // 云台俯仰 (2 bytes, 小端序) - pitch 值 + frame[idx++] = pitch_val & 0xFF; + frame[idx++] = (pitch_val >> 8) & 0xFF; + + // 拨弹轮 (2 bytes, 小端序) + frame[idx++] = feed & 0xFF; + frame[idx++] = (feed >> 8) & 0xFF; + + // 拨杆 (1 byte: 高4位左拨杆，低4位右拨杆) + frame[idx++] = (left_switch << 4) | right_switch; + + // CRC8 (固定为 0xCC) + frame[idx++] = 0xCC; + + // 帧尾 + frame[idx++] = FRAME_TAIL; + + // 调试输出：打印完整帧内容 - 与示例程序一致 + std::string frame_hex; + char buf[4]; + for (int i = 0; i < FRAME_LENGTH; i++) { + snprintf(buf, sizeof(buf), "%02X ", frame[i]); + frame_hex += buf; + } + std::cout << "[TX](" << FRAME_LENGTH << "B): " << frame_hex + << "| pitch=" << pitch_val << std::endl; + + // 发送数据 + ssize_t written = write(fd, frame, FRAME_LENGTH); + if (written != FRAME_LENGTH) { + std::cerr << "发送数据不完整: " << written << "/" << FRAME_LENGTH << std::endl; + } +} + +void printUsage(const char* program) { + std::cout << "用法: " << program << " [pitch角度] [运行秒数]" << std::endl; + std::cout << " pitch角度: 目标角度，正值向上，负值向下，范围建议 (-3, 3) 度" << std::endl; + std::cout << " 运行秒数: 发送时长，0或不填表示无限循环" << std::endl; + std::cout << std::endl; + std::cout << "示例:" << std::endl; + std::cout << " " << program << " # 默认向上1度，无限循环" << std::endl; + std::cout << " " << program << " 1.5 # 向上1.5度，无限循环" << std::endl; + std::cout << " " << program << " -1 5 # 向下1度，运行5秒" << std::endl; + std::cout << " " << program << " 2 10 # 向上2度，运行10秒" << std::endl; +} + +int main(int argc, char* argv[]) { + // 默认参数 + double pitch_angle = -1; // 默认向上1度 + int duration_sec = 0; // 默认无限循环 + + // 解析命令行参数 + if (argc > 1) { + pitch_angle = std::atof(argv[1]); + } + if (argc > 2) { + duration_sec = std::atoi(argv[2]); + } + if (argc > 3) { + printUsage(argv[0]); + return 1; + } + + std::cout << "========================================" << std::endl; + std::cout << "Pitch 轴控制测试程序" << std::endl; + std::cout << "目标角度: " << (pitch_angle > 0 ? "向上 " : "向下 ") + << std::abs(pitch_angle) << " 度" << std::endl; + if (duration_sec > 0) { + std::cout << "运行时长: " << duration_sec << " 秒" << std::endl; + } else { + std::cout << "运行时长: 无限循环 (按 Ctrl+C 停止)" << std::endl; + } + std::cout << "发送频率: 50 Hz" << std::endl; + std::cout << "========================================" << std::endl; + + // 初始化串口 + int fd = initSerial(SERIAL_PORT, BAUDRATE); + if (fd < 0) { + return -1; + } + + // 计算 pitch 值 + // 映射系数 220: 3度 -> 660 + int16_t pitch_val = static_cast(pitch_angle * 220); + if (pitch_val > 660) pitch_val = 660; + if (pitch_val < -660) pitch_val = -660; + + std::cout << "Pitch 发送值: " << pitch_val << std::endl; + std::cout << "----------------------------------------" << std::endl; + + // 计算结束时间 + auto start_time = std::chrono::steady_clock::now(); + auto end_time = start_time + std::chrono::seconds(duration_sec); + + // 持续发送，频率 50Hz + while (true) { + // 检查是否超时 + if (duration_sec > 0 && std::chrono::steady_clock::now() >= end_time) { + std::cout << "运行时间到达 " << duration_sec << " 秒，停止发送" << std::endl; + break; + } + + sendControlFrame(fd, pitch_val); + std::this_thread::sleep_for(std::chrono::milliseconds(20)); // 50Hz + } + + close(fd); + return 0; +} \ No newline at end of file diff --git a/test_yaw_swing b/test_yaw_swing new file mode 100755 index 0000000..a2d0bb9 Binary files /dev/null and b/test_yaw_swing differ diff --git a/test_yaw_swing.cpp b/test_yaw_swing.cpp new file mode 100644 index 0000000..abdf497 --- /dev/null +++ b/test_yaw_swing.cpp @@ -0,0 +1,261 @@ +/** + * @file test_yaw_swing.cpp + * @brief 测试程序：yaw 轴左右摆动测试 + * + * 发送序列: yaw=100 保持1秒 -> yaw=-100 保持1秒 -> 循环 + * 用法: ./test_yaw_swing [循环次数] + * 示例: ./test_yaw_swing 3 (循环3次后停止，默认无限循环) + * + * 协议帧格式（15字节）： + * | 0xBB | 0x77 | x_move(2B) | y_move(2B) | yaw(2B) | pitch(2B) | feed(2B) | switch(1B) | CRC8 | 0xEE | + */ + +#include +#include +#include +#include +#include +#include +#include +#include + +// 帧定义 - 与示例程序完全一致 +constexpr uint8_t FRAME_HEADER_1 = 0xBB; +constexpr uint8_t FRAME_HEADER_2 = 0x77; +constexpr uint8_t FRAME_TAIL = 0xEE; +constexpr int FRAME_LENGTH = 15; + +// 串口配置 +constexpr const char *SERIAL_PORT = "/dev/ttyCH340"; +constexpr int BAUDRATE = 115200; + +// 测试参数 +constexpr int16_t YAW_RIGHT = 100; // 向右100 +constexpr int16_t YAW_LEFT = -100; // 向左-100 +constexpr int HOLD_TIME_MS = 1000; // 保持1秒 +constexpr int SEND_INTERVAL_MS = 20; // 50Hz + +// CRC8 计算 +uint8_t calculateCRC8(const uint8_t *data, size_t len) { + uint8_t crc = 0xFF; + for (size_t i = 0; i < len; i++) { + crc ^= data[i]; + for (int j = 0; j < 8; j++) { + if (crc & 0x80) { + crc = (crc << 1) ^ 0x31; + } else { + crc <<= 1; + } + } + } + return crc; +} + +speed_t convertBaudrate(int baudrate) { + switch (baudrate) { + case 9600: return B9600; + case 19200: return B19200; + case 38400: return B38400; + case 57600: return B57600; + case 115200: return B115200; + default: return B115200; + } +} + +int initSerial(const char *port, int baudrate) { + std::cout << "正在打开串口 " << port << "..." << std::endl; + + int fd = open(port, O_RDWR | O_NOCTTY | O_NDELAY); + if (fd < 0) { + std::cerr << "无法打开串口 " << port << ": " << strerror(errno) << std::endl; + return -1; + } + + struct termios tty; + memset(&tty, 0, sizeof(tty)); + + if (tcgetattr(fd, &tty) != 0) { + std::cerr << "tcgetattr 错误: " << strerror(errno) << std::endl; + close(fd); + return -1; + } + + speed_t baud = convertBaudrate(baudrate); + cfsetospeed(&tty, baud); + cfsetispeed(&tty, baud); + + tty.c_cflag &= ~PARENB; + tty.c_cflag &= ~CSTOPB; + tty.c_cflag &= ~CSIZE; + tty.c_cflag |= CS8; + tty.c_cflag |= CREAD | CLOCAL; + tty.c_cflag &= ~CRTSCTS; + + tty.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG); + tty.c_iflag &= ~(IXON | IXOFF | IXANY); + tty.c_oflag &= ~OPOST; + + tty.c_cc[VMIN] = 0; + tty.c_cc[VTIME] = 1; + + if (tcsetattr(fd, TCSANOW, &tty) != 0) { + std::cerr << "tcsetattr 错误: " << strerror(errno) << std::endl; + close(fd); + return -1; + } + + tcflush(fd, TCIOFLUSH); + std::cout << "串口打开成功" << std::endl; + return fd; +} + +void sendControlFrame(int fd, int16_t yaw_val) { + // 控制参数 + int16_t x_move = 0; + int16_t y_move = 0; + int16_t pitch_val = 0; + int16_t feed = 0; + uint8_t left_switch = 3; + uint8_t right_switch = 3; + + uint8_t frame[FRAME_LENGTH]; + int idx = 0; + + // 帧头 + frame[idx++] = FRAME_HEADER_1; + frame[idx++] = FRAME_HEADER_2; + + // 平动左右 + frame[idx++] = x_move & 0xFF; + frame[idx++] = (x_move >> 8) & 0xFF; + + // 平动前后 + frame[idx++] = y_move & 0xFF; + frame[idx++] = (y_move >> 8) & 0xFF; + + // 云台偏航 (yaw) + frame[idx++] = yaw_val & 0xFF; + frame[idx++] = (yaw_val >> 8) & 0xFF; + + // 云台俯仰 + frame[idx++] = pitch_val & 0xFF; + frame[idx++] = (pitch_val >> 8) & 0xFF; + + // 拨弹轮 + frame[idx++] = feed & 0xFF; + frame[idx++] = (feed >> 8) & 0xFF; + + // 拨杆 + frame[idx++] = (left_switch << 4) | right_switch; + + // CRC8 + frame[idx++] = 0xCC; + + // 帧尾 + frame[idx++] = FRAME_TAIL; + + // 调试输出 + std::string frame_hex; + char buf[4]; + for (int i = 0; i < FRAME_LENGTH; i++) { + snprintf(buf, sizeof(buf), "%02X ", frame[i]); + frame_hex += buf; + } + std::string direction = (yaw_val > 0) ? "右" : "左"; + std::cout << "[TX](" << FRAME_LENGTH << "B): " << frame_hex + << "| yaw=" << yaw_val << " (" << direction << ")" << std::endl; + + ssize_t written = write(fd, frame, FRAME_LENGTH); + if (written != FRAME_LENGTH) { + std::cerr << "发送数据不完整: " << written << "/" << FRAME_LENGTH << std::endl; + } +} + +// 发送指定yaw值，保持指定时间 +void sendYawForDuration(int fd, int16_t yaw_val, int duration_ms) { + auto start = std::chrono::steady_clock::now(); + int count = 0; + + while (true) { + auto now = std::chrono::steady_clock::now(); + auto elapsed = std::chrono::duration_cast(now - start).count(); + + if (elapsed >= duration_ms) { + break; + } + + sendControlFrame(fd, yaw_val); + count++; + std::this_thread::sleep_for(std::chrono::milliseconds(SEND_INTERVAL_MS)); + } + + std::cout << " 发送了 " << count << " 帧，时长 " << duration_ms << "ms" << std::endl; +} + +void printUsage(const char* program) { + std::cout << "用法: " << program << " [循环次数]" << std::endl; + std::cout << " 循环次数: 测试循环次数，0或不填表示无限循环" << std::endl; + std::cout << std::endl; + std::cout << "示例:" << std::endl; + std::cout << " " << program << " # 无限循环 (按 Ctrl+C 停止)" << std::endl; + std::cout << " " << program << " 5 # 循环5次后停止" << std::endl; +} + +int main(int argc, char* argv[]) { + int loop_count = 0; // 0表示无限循环 + int current_loop = 0; + + if (argc > 1) { + loop_count = std::atoi(argv[1]); + } + if (argc > 2) { + printUsage(argv[0]); + return 1; + } + + std::cout << "========================================" << std::endl; + std::cout << "Yaw 轴摆动测试程序" << std::endl; + std::cout << "测试序列: yaw=" << YAW_RIGHT << "(右,1s) -> yaw=" << YAW_LEFT << "(左,1s)" << std::endl; + if (loop_count > 0) { + std::cout << "循环次数: " << loop_count << " 次" << std::endl; + } else { + std::cout << "循环次数: 无限 (按 Ctrl+C 停止)" << std::endl; + } + std::cout << "发送频率: 50 Hz" << std::endl; + std::cout << "========================================" << std::endl; + + // 初始化串口 + int fd = initSerial(SERIAL_PORT, BAUDRATE); + if (fd < 0) { + return -1; + } + + std::cout << "----------------------------------------" << std::endl; + + // 主循环 + while (true) { + if (loop_count > 0 && current_loop >= loop_count) { + std::cout << "完成 " << loop_count << " 次循环，测试结束" << std::endl; + break; + } + + current_loop++; + if (loop_count > 0) { + std::cout << "\n=== 第 " << current_loop << "/" << loop_count << " 次循环 ===" << std::endl; + } else { + std::cout << "\n=== 第 " << current_loop << " 次循环 ===" << std::endl; + } + + // 步骤1: yaw=100 (向右) 保持1秒 + std::cout << "【步骤1】yaw=" << YAW_RIGHT << " (向右) 保持 " << HOLD_TIME_MS << "ms" << std::endl; + sendYawForDuration(fd, YAW_RIGHT, HOLD_TIME_MS); + + // 步骤2: yaw=-100 (向左) 保持1秒 + std::cout << "【步骤2】yaw=" << YAW_LEFT << " (向左) 保持 " << HOLD_TIME_MS << "ms" << std::endl; + sendYawForDuration(fd, YAW_LEFT, HOLD_TIME_MS); + } + + close(fd); + std::cout << "串口已关闭" << std::endl; + return 0; +} \ No newline at end of file diff --git a/tools/monitor.bat b/tools/monitor.bat index e8fa909..c725e1c 100644 --- a/tools/monitor.bat +++ b/tools/monitor.bat @@ -1,6 +1,6 @@ @echo off -rem سĽͳ·ɸҪ޸ +rem 定义需监控程序的进程名和程序路径，可根据需要进行修改 set AppName=run.exe @@ -8,43 +8,43 @@ set AppArgs= --run-with-camera --wait-uart --show-armor-box set AppPath=C:\Users\sjturm\Desktop\AutoAim\build\Release\ -title ̼ +title 进程监控 cls echo. -echo ̼ؿʼ +echo 进程监控开始…… echo. -rem ѭ +rem 定义循环体 :startjc - rem ӽбвָ + rem 从进程列表中查找指定进程 - rem Ҳд qprocess %AppName% >nul 鷢󲹳䣩 + rem 下面语句也可写成 qprocess %AppName% >nul （经验发布后补充） qprocess|findstr /i %AppName% >nul - rem errorlevelֵ0ʾҵ̣ûвҵ + rem 变量errorlevel的值等于0表示查找到进程，否则没有查找到进程 if %errorlevel%==0 ( - echo ^>%date:~0,10% %time:~0,8% С + echo ^>%date:~0,10% %time:~0,8% 程序正在运行…… )else ( - echo ^>%date:~0,10% %time:~0,8% ûзֳ + echo ^>%date:~0,10% %time:~0,8% 没有发现程序进程 - echo ^>%date:~0,10% %time:~0,8% + echo ^>%date:~0,10% %time:~0,8% 正在重新启动程序 - start %AppPath%%AppName%%AppArgs% 2>nul && echo ^>%date:~0,10% %time:~0,8% ɹ + start %AppPath%%AppName%%AppArgs% 2>nul && echo ^>%date:~0,10% %time:~0,8% 启动程序成功 ) - rem pingʵʱ + rem 用ping命令来实现延时运行 ping -n 2 -w 1000 1.1.1.1>nul