fallback CMake + Armor PID

armor/include/armor_finder/armor_finder.h

@@ -120,6 +120,10 @@ private:
     vector<systime> time_seq;                           // 一个周期内的时间采样点
     vector<float> angle_seq;                            // 一个周期内的角度采样点
 
+    float yaw_rotation, pitch_rotation;//云台yaw轴和pitch轴应该转到的角度
+    float last_yaw, last_pitch;//PID中微分项
+    float sum_yaw, sum_pitch;//yaw和pitch的累计误差，即PID中积分项
+
     bool findLightBlobs(const cv::Mat &src, LightBlobs &light_blobs);
     bool findArmorBox(const cv::Mat &src, ArmorBox &box);
     bool matchArmorBoxes(const cv::Mat &src, const LightBlobs &light_blobs, ArmorBoxes &armor_boxes);

armor/src/armor_finder/send_target/send_target.cpp

@@ -22,6 +22,8 @@ static bool sendTarget(Serial &serial, double x, double y, double z, uint16_t sh
     fps += 1;
 #endif
 
+#define MINMAX(value, min, max) value = ((value) < (min)) ? (min) : ((value) > (max) ? (max) : (value))
+
     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);
@@ -49,6 +51,24 @@ 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;
+
+    // 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;
+
+    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;
+    //
+
     double yaw = atan(dx / FOCUS_PIXAL) * 180 / PI;
     double pitch = atan(dy / FOCUS_PIXAL) * 180 / PI;
     double dist = DISTANCE_HEIGHT / rect.height;