有vofa验证

armor/src/armor_finder/send_target/send_target.cpp

@@ -7,6 +7,7 @@
 #include <log.h>
 
 static bool sendTarget(Serial &serial, double x, double y, double z, uint16_t shoot_delay) {
+    
     static short x_tmp, y_tmp, z_tmp;
     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))
 
-    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);
     z_tmp = static_cast<short>(z * (32768 - 1) / 1000);
     
+
     buff[0] = 's';
     buff[1] = static_cast<char>((x_tmp >> 8) & 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[8] = static_cast<char>((shoot_delay >> 0) & 0xFF);
     buff[9] = 'e';
-//    if(buff[7]<<8 | buff[8])
-//        cout << (buff[7]<<8 | buff[8]) << endl;
+    //if(buff[7]<<8 | buff[8])
+    //   cout << (buff[7]<<8 | buff[8]) << endl;
     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) {
     if (target_box.rect == cv::Rect2d()) return false;
-    if (shoot_delay) {
+    /*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
+    #define MAX_YAW 100
+    #define MAX_PITCH 80
     sum_yaw += dx;
     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 pitch_I_component = PITCH_AIM_KI * sum_pitch;