chore: 导入 Simba-OmniInfantry 基线(云台/底盘双工程)

- 云台(ROBOT_TYPE_GIMBAL)与底盘(ROBOT_TYPE_CHASSIS)完整固件源码 - 新增根级 .gitignore,排除 Keil MDK-ARM 构建产物(.o/.d/.crf/.axf/.lnp/.map/.htm/.sct/.dep/.iex/.lst 等) 与 *.uvguix.* / DebugConfig/(与机器绑定的 UI 布局) - core.autocrlf=false,避免固件源码在检出时被改动换行符
2026-09-12 10:28:21 +08:00
commit a4da5ad17c
517 changed files with 503795 additions and 0 deletions
@@ -0,0 +1,126 @@
+/* =============================================================================
+ * pid.c - 位置式/增量式 PID(自 legacy Control/PID 原样移植)
+ * 用法:
+ *   PID_PositionStructureInit(pid, need)             初始化
+ *   PID_PositionSetParameter(pid, kp, ki, kd)        设置增益
+ *   PID_PositionSetEkRange(pid, lo, hi)              误差死区(|ek|在区间内视为0)
+ *   PID_PositionSetOUTRange(pid, lo, hi)             输出限幅
+ *   PID_PositionCalc(pid, now)                       每周期计算,结果在 pid->OUT
+ *   PID_PositionClean(pid)                           清误差与积分(模式切换时)
+ * 注意: 积分无独立限幅,靠输出限幅+死区约束;所有云台/底盘 PID 参数集中在
+ *       robot_param.h,控制周期 2ms(与 legacy 调参时一致,勿随意改周期)
+ * ========================================================================== */
+#include "pid.h"
+
+void PID_PositionStructureInit(PID_PositionTypeDef * pid, float need_value)
+{
+    pid->Need_Value = need_value;
+    pid->Ek = 0;
+    pid->Sum_Ek = 0;
+    pid->Ek_low = 0;
+    pid->Ek_up = 0;
+    pid->Kp = 0;
+    pid->Ki = 0;
+    pid->Kd = 0;
+    pid->OUT_low = -1e10;
+    pid->OUT_up = 1e10;
+}
+
+void PID_PositionSetParameter(PID_PositionTypeDef * pid, float kp, float ki, float kd)
+{
+    pid->Kp = kp;
+    pid->Ki = ki;
+    pid->Kd = kd;
+}
+
+void PID_PositionSetEkRange(PID_PositionTypeDef * pid, float ek_low, float ek_up)
+{
+    pid->Ek_low = ek_low;
+    pid->Ek_up = ek_up;
+}
+
+void PID_PositionSetOUTRange(PID_PositionTypeDef * pid, float out_low, float out_up)
+{
+    pid->OUT_low = out_low;
+    pid->OUT_up = out_up;
+}
+
+void PID_PositionClean(PID_PositionTypeDef * pid)
+{
+    pid->Ek = 0;
+    pid->Sum_Ek = 0;
+}
+
+void PID_PositionCalc(PID_PositionTypeDef * pid, float now_value)
+{
+    pid->Now_Value = now_value;
+    pid->Ek_1 = pid->Ek;
+    pid->Ek = pid->Need_Value - pid->Now_Value;
+    if (pid->Ek_low < pid->Ek && pid->Ek < pid->Ek_up)  // 误差为0检测
+        pid->Ek = 0;
+    pid->Sum_Ek += pid->Ek;
+    pid->Del_Ek = pid->Ek - pid->Ek_1;
+
+    pid->P_OUT = pid->Kp * pid->Ek;
+    pid->I_OUT = pid->Ki * pid->Sum_Ek;
+    pid->D_OUT = pid->Kd * pid->Del_Ek;
+    pid->OUT = pid->P_OUT + pid->I_OUT + pid->D_OUT;
+
+    if (pid->OUT < pid->OUT_low)  // 输出限幅
+        pid->OUT = pid->OUT_low;
+    if (pid->OUT > pid->OUT_up)
+        pid->OUT = pid->OUT_up;
+}
+
+void PID_IncrementalStructureInit(PID_IncrementalTypeDef * pid, float need_value)
+{
+    pid->Need_Value = need_value;
+    pid->Ek = 0;
+    pid->Ek_1 = 0;
+    pid->Ek_low = 0;
+    pid->Ek_up = 0;
+    pid->Kp = 0;
+    pid->Ki = 0;
+    pid->Kd = 0;
+    pid->OUT_low = -1e10;
+    pid->OUT_up = 1e10;
+}
+
+void PID_IncrementalSetParameter(PID_IncrementalTypeDef * pid, float kp, float ki, float kd)
+{
+    pid->Kp = kp;
+    pid->Ki = ki;
+    pid->Kd = kd;
+}
+
+void PID_IncrementalSetEkRange(PID_IncrementalTypeDef * pid, float ek_low, float ek_up)
+{
+    pid->Ek_low = ek_low;
+    pid->Ek_up = ek_up;
+}
+
+void PID_IncrementalSetOUTRange(PID_IncrementalTypeDef * pid, float out_low, float out_up)
+{
+    pid->OUT_low = out_low;
+    pid->OUT_up = out_up;
+}
+
+void PID_IncrementalCalc(PID_IncrementalTypeDef * pid, float now_value)
+{
+    pid->Now_Value = now_value;
+    pid->Ek_2 = pid->Ek_1;
+    pid->Ek_1 = pid->Ek;
+    pid->Ek = pid->Need_Value - pid->Now_Value;
+    if (pid->Ek_low < pid->Ek && pid->Ek < pid->Ek_up)  // 误差为0检测
+        pid->Ek = 0;
+
+    pid->P_OUT = pid->Kp * (pid->Ek - pid->Ek_1);
+    pid->I_OUT = pid->Ki * pid->Ek;
+    pid->D_OUT = pid->Kd * (pid->Ek - 2 * pid->Ek_1 + pid->Ek_2);
+    pid->OUT = pid->P_OUT + pid->I_OUT + pid->D_OUT;
+
+    if (pid->OUT < pid->OUT_low)  // 输出限幅
+        pid->OUT = pid->OUT_low;
+    if (pid->OUT > pid->OUT_up)
+        pid->OUT = pid->OUT_up;
+}
@@ -0,0 +1,66 @@
+#ifndef PID_H
+#define PID_H
+
+/* 位置式/增量式 PID,自 legacy 工程 Control/PID 原样移植 */
+
+typedef struct
+{
+    float Need_Value;  // 预期值
+    float Now_Value;   // 当前值
+
+    float Ek;      // 本次误差
+    float Ek_1;    // 上一次误差
+    float Sum_Ek;  // 误差积分
+    float Del_Ek;  // 误差差分
+    float Ek_low;  // 误差为0阈值下限
+    float Ek_up;   // 误差为0阈值上限
+
+    float Kp;  // Kp
+    float Ki;  // Ki
+    float Kd;  // Kd
+
+    float P_OUT;  // 比例输出
+    float I_OUT;  // 积分输出
+    float D_OUT;  // 微分输出
+    float OUT;    // 总输出
+    float OUT_low;  // 输出限幅下限
+    float OUT_up;   // 输出限幅上限
+} PID_PositionTypeDef;  // 位置式PID参数结构体
+
+typedef struct
+{
+    float Need_Value;  // 预期值
+    float Now_Value;   // 当前值
+
+    float Ek;     // 本次误差
+    float Ek_1;   // 上一次误差
+    float Ek_2;   // 上两次误差
+    float Ek_low;  // 误差为0阈值下限
+    float Ek_up;   // 误差为0阈值上限
+
+    float Kp;  // Kp
+    float Ki;  // Ki
+    float Kd;  // Kd
+
+    float P_OUT;  // 比例输出
+    float I_OUT;  // 积分输出
+    float D_OUT;  // 微分输出
+    float OUT;    // 总输出
+    float OUT_low;  // 输出限幅下限
+    float OUT_up;   // 输出限幅上限
+} PID_IncrementalTypeDef;  // 增量式PID参数结构体
+
+void PID_PositionStructureInit(PID_PositionTypeDef * pid, float need_value);
+void PID_PositionSetParameter(PID_PositionTypeDef * pid, float kp, float ki, float kd);
+void PID_PositionSetEkRange(PID_PositionTypeDef * pid, float ek_low, float ek_up);
+void PID_PositionSetOUTRange(PID_PositionTypeDef * pid, float out_low, float out_up);
+void PID_PositionClean(PID_PositionTypeDef * pid);
+void PID_PositionCalc(PID_PositionTypeDef * pid, float now_value);
+
+void PID_IncrementalStructureInit(PID_IncrementalTypeDef * pid, float need_value);
+void PID_IncrementalSetParameter(PID_IncrementalTypeDef * pid, float kp, float ki, float kd);
+void PID_IncrementalSetEkRange(PID_IncrementalTypeDef * pid, float ek_low, float ek_up);
+void PID_IncrementalSetOUTRange(PID_IncrementalTypeDef * pid, float out_low, float out_up);
+void PID_IncrementalCalc(PID_IncrementalTypeDef * pid, float now_value);
+
+#endif