yaw中心轴

armor/include/armor_finder/armor_finder.h

@@ -120,9 +120,10 @@ private:
 
     systime last_front_time;                            // 上次陀螺正对时间
     int anti_top_cnt;
-    RoundQueue<double, 4> top_periodms;                 // 陀螺周期循环队列
-    vector<systime> time_seq;                           // 一个周期内的时间采样点
-    vector<float> angle_seq;                            // 一个周期内的角度采样点
+    double auto_omega;                                  // 角速度缓存
+    double last_phase;                                  // 上次相位角
+    systime last_phase_time;                            // 上次相位角时间
+
 
     float yaw_rotation, pitch_rotation;//云台yaw轴和pitch轴应该转到的角度
     float last_yaw, last_pitch;//PID中微分项
@@ -140,6 +141,7 @@ private:
     cv::Point3f getTarget3D(const ArmorBox &box);      // 获取目标的3D坐标 (相对于相机)
 
     bool sendBoxPosition(uint16_t shoot_delay);           // 发送装甲板位置
+    bool sendAntiTopTarget(double yaw, uint16_t shoot_delay, bool fire); // 发送反陀螺射击目标
     bool shouldFire() const { return can_fire; }        // 获取开火建议
 public:
     void run(cv::Mat &src);                             // 自瞄主函数