ELCT/Simba-OmniInfantry
9
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

写入了发弹检测模块，并为电流环控制

Browse Source
This commit is contained in:
lyf
2026-10-12 20:19:45 +08:00
parent 67eea77789
commit 4427157df7
14 changed files with 760 additions and 83 deletions
Download Patch File Download Diff File Expand all files Collapse all files
云台/Application/robot_param.h
+27 -14
View File
@@ -39,28 +39,39 @@
#define GIMBAL_YAW_ANGLE_EK_DEADBAND 0.05f // 误差死区(°),QEKF 噪声小可用窄死区
#define GIMBAL_YAW_ANGLE_OUT_LIMIT 360.0f // 目标角速度上限(°/s)
#define GIMBAL_YAW_SPEED_KP 85.0f // °/s -> 电压:100°/s 误差给 6000(约 24% 满电压)
#define GIMBAL_YAW_SPEED_KP 85.0f // °/s -> 电流:需根据实际电流环响应微调
#define GIMBAL_YAW_SPEED_KI 0.0f // 稳定后可回填 0.5~2 消速度稳态差
#define GIMBAL_YAW_SPEED_KD 0.0f // 陀螺速率反馈噪声小,先不用 D
#define GIMBAL_YAW_SPEED_OUT_LIMIT 25000.0f// GM6020 电压限幅
#define GIMBAL_YAW_SPIN_FEEDFORWARD_K 24.0f // 小陀螺底盘自旋解耦前馈系数(RPM->电压,通常15~35)
#define GIMBAL_YAW_SPEED_OUT_LIMIT 16384.0f// GM6020 电流限幅(±16384 对应 ±3A)
#define GIMBAL_YAW_SPIN_FEEDFORWARD_K 16.0f // 小陀螺底盘自旋解耦前馈系数(RPM->电流)
/* Yaw 装配符号自动检测:上电自检时给 Yaw 电机一个正向小电压脉冲,
/* Yaw 装配符号自动检测:上电自检时给 Yaw 电机一个正向小电流脉冲,
* 看陀螺 z 轴往哪边转——脉冲引起同向转动则符号 +1,反向则 -1。
* 检测时云台会轻转几度,勿扶云台;失败(被按住/电机离线)时用 FALLBACK */
#define GIMBAL_YAW_SIGN_AUTO 1 // 1=上电自动检测 0=直接用 FALLBACK
#define GIMBAL_YAW_SIGN_FALLBACK (1.0f)
#define GIMBAL_YAW_DETECT_VOLTAGE 5000 // 检测脉冲电压(约 20% 满电压)
#define GIMBAL_YAW_DETECT_CURRENT 3500 // 检测脉冲电流(±16384 中约 21% 满量程,约 0.64A)
#define GIMBAL_YAW_DETECT_VOLTAGE GIMBAL_YAW_DETECT_CURRENT // 兼容旧宏
#define GIMBAL_YAW_DETECT_RATE_MIN 20.0f // 判定转动的陀螺速率阈值(°/s)
/* ---- Pitch(位置环陀螺 Roll,速度环转子转速——Pitch 无符号史问题,维持原案) ---- */
#define GIMBAL_PITCH_ANGLE_EK_DEADBAND 0.3f // 位置环误差死区(°)
#define GIMBAL_PITCH_ANGLE_KP 15.0f
/* ---- Pitch 轴(电流环控制模式): 位置环陀螺 Roll, 速度环电机转子转速 ----
* 参数适配 GM6020 电流环(±16384 对应 ±3A):
* - 速度环: 单位为 RPM -> 电流, KP/KD 适度折算抑制高频啸叫抖动,
* KI 适度增强以克服云台偏心重力矩下垂静差, 缩小死区避免悬停点头;
* - 位置环: 角度误差(°) -> 期望转速(RPM), 限制期望转速以平顺控制。
*/
#define GIMBAL_PITCH_ANGLE_EK_DEADBAND 0.25f // 位置环误差死区(°)
#define GIMBAL_PITCH_ANGLE_KP 15.0f // 10° 误差输出 150 RPM 期望速度
#define GIMBAL_PITCH_ANGLE_KI 0.0f
#define GIMBAL_PITCH_ANGLE_KD 100.0f
#define GIMBAL_PITCH_SPEED_KP 150.0f
#define GIMBAL_PITCH_SPEED_KI 0.001f
#define GIMBAL_PITCH_SPEED_KD 5.0f
#define GIMBAL_PITCH_ANGLE_KD 20.0f // 位置环微分阻尼
#define GIMBAL_PITCH_ANGLE_OUT_LIMIT 120.0f // 期望转速上限(RPM, 120 RPM = 720°/s)
#define GIMBAL_PITCH_SPEED_KP 75.0f // 速度环 KP: 100 RPM 误差给 7500(约 45% 额定转矩)
#define GIMBAL_PITCH_SPEED_KI 0.05f // 速度环 KI: 消除重力静态下垂与倾角误差
#define GIMBAL_PITCH_SPEED_KD 1.5f // 速度环 KD: 抑制电流噪声与轻微颤动
#define GIMBAL_PITCH_SPEED_EK_DEADBAND 0.5f // 速度环死区(RPM, 电流环需窄死区防悬停下垂)
#define GIMBAL_PITCH_SPEED_OUT_LIMIT 16384.0f// GM6020 电流限幅(±16384 对应 ±3A)
#define GIMBAL_PITCH_GRAVITY_COMP 0.0f // 重力前馈偏置电流(枪管偏重时可设 500~1500)
#define GIMBAL_FRICTION_WHEEL_KP 16.0f
#define GIMBAL_FRICTION_WHEEL_KI 0.0f
@@ -119,8 +130,10 @@
#define CTOC_SLAVE_ID3 0x1A9 // 云台->底盘:云台世界系 yaw(float)+att_ready
/* ============================ CAN ID ============================ */
/* 云台板:CAN1 = Pitch GM6020(0x206)+摩擦轮 M3508(0x201/0x202)
* CAN2 = Yaw GM6020(0x205)+底盘回传(0x019)
#define GIMBAL_RAMMER_CAN_ID 0x201 // 拨弹盘 M2006 反馈 ID(CAN2 0x200 控制帧槽 1, 对应电机拨码 ID 1)
/* 云台板:CAN1 = Pitch GM6020(0x206) + 摩擦轮 M3508(0x201/0x202)
* CAN2 = Yaw GM6020(0x205) + 拨弹 M2006(0x201) + 底盘回传(0x019)
* 底盘板:CAN1 = 四轮 M3508(0x201~0x204)
* CAN2 = 云台数据(0x149/0x189/0x1A9) */