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

有vofa验证

Browse Source
This commit is contained in:
Huang Haoyu
2026-03-28 04:26:42 +08:00
parent 460ef760fa
commit 9104d65bb2
1 changed files with 21 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -7,6 +7,7 @@
#include <log.h> #include <log.h>
static bool sendTarget(Serial &serial, double x, double y, double z, uint16_t shoot_delay) { static bool sendTarget(Serial &serial, double x, double y, double z, uint16_t shoot_delay) {
static short x_tmp, y_tmp, z_tmp; static short x_tmp, y_tmp, z_tmp;
uint8_t buff[10]; uint8_t buff[10];
@@ -24,10 +25,11 @@ static bool sendTarget(Serial &serial, double x, double y, double z, uint16_t sh
#define MINMAX(value, min, max) value = ((value) < (min)) ? (min) : ((value) > (max) ? (max) : (value)) #define MINMAX(value, min, max) value = ((value) < (min)) ? (min) : ((value) > (max) ? (max) : (value))
x_tmp = static_cast<short>(x * (32768 - 1) / 100); x_tmp = -static_cast<short>(x * (32768 - 1) / 100);
y_tmp = static_cast<short>(y * (32768 - 1) / 100); y_tmp = static_cast<short>(y * (32768 - 1) / 100);
z_tmp = static_cast<short>(z * (32768 - 1) / 1000); z_tmp = static_cast<short>(z * (32768 - 1) / 1000);
buff[0] = 's'; buff[0] = 's';
buff[1] = static_cast<char>((x_tmp >> 8) & 0xFF); buff[1] = static_cast<char>((x_tmp >> 8) & 0xFF);
buff[2] = static_cast<char>((x_tmp >> 0) & 0xFF); buff[2] = static_cast<char>((x_tmp >> 0) & 0xFF);
@@ -38,23 +40,37 @@ static bool sendTarget(Serial &serial, double x, double y, double z, uint16_t sh
buff[7] = static_cast<char>((shoot_delay >> 8) & 0xFF); buff[7] = static_cast<char>((shoot_delay >> 8) & 0xFF);
buff[8] = static_cast<char>((shoot_delay >> 0) & 0xFF); buff[8] = static_cast<char>((shoot_delay >> 0) & 0xFF);
buff[9] = 'e'; buff[9] = 'e';
// if(buff[7]<<8 | buff[8]) //if(buff[7]<<8 | buff[8])
// cout << (buff[7]<<8 | buff[8]) << endl; // cout << (buff[7]<<8 | buff[8]) << endl;
return serial.WriteData(buff, sizeof(buff)); return serial.WriteData(buff, sizeof(buff));
// Vofa串口验证
//char buff[128];
//int len = (buff, "s %lf %lf %lf %d e", x, y, z, shoot_delay);
//return serial.WriteData((unsigned char *)buff, len);
} }
bool ArmorFinder::sendBoxPosition(uint16_t shoot_delay) { bool ArmorFinder::sendBoxPosition(uint16_t shoot_delay) {
if (target_box.rect == cv::Rect2d()) return false; if (target_box.rect == cv::Rect2d()) return false;
if (shoot_delay) { /*if (shoot_delay) {
LOGM(STR_CTR(WORD_BLUE, "next box %dms"), shoot_delay); LOGM(STR_CTR(WORD_BLUE, "next box %dms"), 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;
// PID // PID
#define MAX_YAW 100
#define MAX_PITCH 80
sum_yaw += dx; sum_yaw += dx;
sum_pitch += dy; sum_pitch += dy;
sum_yaw =(sum_yaw > MAX_YAW) ? MAX_YAW : (sum_yaw < -MAX_YAW ? -MAX_YAW : sum_yaw);
sum_pitch =(sum_pitch > MAX_PITCH) ? MAX_PITCH : (sum_pitch < -MAX_PITCH ? -MAX_PITCH : sum_pitch);
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;