电流环控制，并实现简单的补偿控制，还差参数具体化。
This commit is contained in:
@@ -9,10 +9,10 @@
|
||||
#define ROBOT_PARAM_H
|
||||
|
||||
/* ============================ 云台 ============================ */
|
||||
#define PITCH_GM6020_POSITION_LOWER_LIMIT 3990 // Pitch 轴编码器值下限(云台抬起方向)
|
||||
#define PITCH_GM6020_POSITION_UPPER_LIMIT 2220 // Pitch 轴编码器值上限(云台压低方向)
|
||||
#define PITCH_GM6020_ENCODER_ZERO 3245 // Pitch 轴编码器零位
|
||||
#define YAW_GM6020_ENCODER_ZERO 6804 // Yaw 轴编码器零位(机械正前方,0~8191)
|
||||
#define PITCH_GM6020_POSITION_UPPER_LIMIT 2260 // Pitch 轴抬起极限(最高位,编码器较小值;实测2203撞死限位,留余量设2260)
|
||||
#define PITCH_GM6020_POSITION_LOWER_LIMIT 3950 // Pitch 轴压低极限(最低位,编码器较大值;实测4234撞死限位,留余量设3950)
|
||||
#define PITCH_GM6020_ENCODER_ZERO 3200 // Pitch 轴编码器水平零位
|
||||
#define YAW_GM6020_ENCODER_ZERO 7730 // Yaw 轴编码器零位(机械正前方,0~8191)
|
||||
#define CHASSIS_SPIN_FEEDFORWARD_K 0.008f // 小陀螺解算时延前馈补偿系数(s)
|
||||
|
||||
#define GIMBAL_FRICTION_WHEEL_SPEED 5500.0f // 摩擦轮转速(RPM)
|
||||
@@ -31,7 +31,7 @@
|
||||
* 这是反复"疯转"的根源,已废弃;
|
||||
* - 现在两环共用陀螺,整条链只有一个装配符号,由上电自检自动测定
|
||||
* (GIMBAL_YAW_DETECT 见下),无需人工猜;
|
||||
* - 单位:角度环 误差° -> 目标角速度°/s;速度环 误差°/s -> 电压。
|
||||
* - 单位:角度环 误差° -> 目标角速度°/s;速度环 误差°/s -> 电流。
|
||||
* 调参顺序:先速度环(点动不振荡),再位置环(刚性)——见移植 README §8.1 */
|
||||
#define GIMBAL_YAW_ANGLE_KP 20.0f // ° -> °/s:10° 误差给 150°/s
|
||||
#define GIMBAL_YAW_ANGLE_KI 0.0f
|
||||
@@ -39,28 +39,50 @@
|
||||
#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 -> 电流:100°/s 误差给 6000(约 36% 满电流)
|
||||
#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 24.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 // 检测脉冲电流(约 21% 满电流 16384)
|
||||
#define GIMBAL_YAW_DETECT_RATE_MIN 20.0f // 判定转动的陀螺速率阈值(°/s)
|
||||
|
||||
/* ---- Pitch(位置环陀螺 Roll,速度环转子转速——Pitch 无符号史问题,维持原案) ---- */
|
||||
#define GIMBAL_PITCH_ANGLE_EK_DEADBAND 0.3f // 位置环误差死区(°)
|
||||
#define GIMBAL_PITCH_ANGLE_EK_DEADBAND 0.05f // 误差死区(°),收窄死区消除边缘阶跃
|
||||
#define GIMBAL_PITCH_ANGLE_KP 15.0f
|
||||
#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 0.0f // 原100.0f是万级电流振颤主因!清零以消除剧烈振荡
|
||||
#define GIMBAL_PITCH_SPEED_KP 40.0f // 速度/电流环Kp
|
||||
#define GIMBAL_PITCH_SPEED_KI 0.0f
|
||||
#define GIMBAL_PITCH_SPEED_KD 0.0f
|
||||
|
||||
/* Pitch 重力拟合前馈参数: I_g = K_COS * cos(a) + K_SIN * sin(a) + K_CONST
|
||||
* 其中 a = (raw_angle - ZERO_ECD) * 360 / 8192 (°) */
|
||||
#define GIMBAL_PITCH_GRAVITY_ZERO_ECD 3200.0f // 重力补偿拟合基准编码值
|
||||
#define GIMBAL_PITCH_GRAVITY_K_COS 2856.1f// cos 项系数
|
||||
#define GIMBAL_PITCH_GRAVITY_K_SIN 4340.3f// sin 项系数
|
||||
#define GIMBAL_PITCH_GRAVITY_K_CONST 2826.5f // 常数偏置项
|
||||
|
||||
/* ---- 2026-10-15 台架前馈实测记录(未采用,保留备查) ----
|
||||
* 用上位机 203Hz 遥测在编码器 -16.91°/+4.88°/+9.30° 三处长时间静止(编码器
|
||||
* 零漂移+目标恒定)实测真实保持电流 3597/4498/4613 counts;当时上面这组参数
|
||||
* 分别高估 699/1543/1733 counts,折合 1.17~2.89° 稳态静差
|
||||
* (位置环 Kp15×Kp40 = 600 counts/°:静差 = 前馈高估量 / 600)。
|
||||
* 按"保留原相位与形状(K_COS:K_SIN 不变)、幅值×0.503 并微调常数项"重标定为:
|
||||
* GIMBAL_PITCH_GRAVITY_K_COS 1435.4f
|
||||
* GIMBAL_PITCH_GRAVITY_K_SIN 2181.3f
|
||||
* GIMBAL_PITCH_GRAVITY_K_CONST 2861.6f
|
||||
* 该组在当次数据上残差 RMS 16 counts(0.03°),看似很准;**但换到另一机械状态后
|
||||
* 同一编码器角度实测保持电流又差约 1650 counts**,说明被标定对象本身在变
|
||||
* (见上方"机械滞回 2130 counts"记录),故不采用,前馈保持原值不动。
|
||||
* 另一并记录:PID 动态特性实测良好(上升时间 0.095~0.14s、超调 0.3~5.2%、
|
||||
* 16s 稳态段纹波 p-p 0.046°、无振荡),故 GIMBAL_PITCH_*_KP/KI/KD 也未改动。 */
|
||||
|
||||
#define GIMBAL_FRICTION_WHEEL_KP 16.0f
|
||||
#define GIMBAL_FRICTION_WHEEL_KI 0.0f
|
||||
@@ -119,8 +141,8 @@
|
||||
#define CTOC_SLAVE_ID3 0x1A9 // 云台->底盘:云台世界系 yaw(float)+att_ready
|
||||
|
||||
/* ============================ CAN ID ============================ */
|
||||
/* 云台板:CAN1 = Pitch GM6020(0x206)+摩擦轮 M3508(0x201/0x202)
|
||||
* CAN2 = Yaw GM6020(0x205)+底盘回传(0x019)
|
||||
/* 云台板:CAN1 = Pitch GM6020(0x206,0x1FE)+摩擦轮 M3508(0x201/0x202,0x200)
|
||||
* CAN2 = Yaw GM6020(0x205,0x1FE)+拨弹 M2006(0x201,0x200)+底盘回传(0x019)
|
||||
* 底盘板:CAN1 = 四轮 M3508(0x201~0x204)
|
||||
* CAN2 = 云台数据(0x149/0x189/0x1A9) */
|
||||
|
||||
@@ -128,6 +150,41 @@
|
||||
#define DEBUG_SHOOT_WITHOUT_REFEREE 1 // 1=无裁判系统时也允许开摩擦轮(台架调试用),比赛请改 0
|
||||
#define GIMBAL_RAMMER_INSTALLED 0 // 1=拨弹 M2006 已安装;0=暂缺,不参与掉线判定
|
||||
|
||||
/* ================= 2026-10-15 Pitch 台架 PID 调参改动(已停用,保留备查) =================
|
||||
* 以下开关是一次台架 Pitch 调参试验所加,已用 #if 0 停用:不参与编译、不影响现有行为。
|
||||
* 需要复现时把外层 #if 0 改成 #if 1,并同步启用 gimbal.c / gimbal_task.c / gimbal.h
|
||||
* / debug_task.c 里同名的停用块。
|
||||
* 启用前务必先确认机械无卡滞:当时实测**同一编码器角度下保持电流可差 2130 counts
|
||||
* (≈3.55° 当量)**,属机械滞回(线缆拖拽/接触/摩擦),不是控制问题;机械不一致时
|
||||
* 任何固定前馈都标不准,调参也无意义。 */
|
||||
#if 0
|
||||
/* ---- Pitch 台架 PID 调参(仅台架;调完/比赛必须置 0) ----
|
||||
* 1=在 Pitch 位置环设定值上叠加一个方波阶跃,用于定量观测阶跃响应。
|
||||
* 安全约束(务必遵守):
|
||||
* - **有界**:只跑 GIMBAL_BENCH_PITCH_HALF_COUNT 个半周期就自动归零停住,
|
||||
* 不做长时间连续抖动;
|
||||
* - **限位内**:轴距上下软限位不足 GIMBAL_BENCH_PITCH_SAFE_MARGIN_DEG 时
|
||||
* 不施加阶跃,绝不把轴推向限位;
|
||||
* - 该偏置是纯附加量,不累加进 pitch_target_deg,关掉即完全恢复原行为。 */
|
||||
#define GIMBAL_BENCH_PITCH_TUNE 0
|
||||
#define GIMBAL_BENCH_PITCH_STEP_DEG 3.0f // 阶跃幅值(°)
|
||||
#define GIMBAL_BENCH_PITCH_HALF_MS 800 // 半周期(ms),(跑完自动停,非持续抖动)
|
||||
#define GIMBAL_BENCH_PITCH_HALF_COUNT 4 // 总半周期数:4×0.8s=3.2s 后自动静止
|
||||
#define GIMBAL_BENCH_PITCH_SAFE_MARGIN_DEG 5.0f // 距上下限位至少留这么多才允许阶跃
|
||||
|
||||
/* Pitch 软限位外的电流限制(方向 + 堵转双层保护):
|
||||
* - 压向限位的方向一律清零(绝不顶限位);
|
||||
* - 离开限位的方向允许 GIMBAL_PITCH_LIMIT_CURRENT_RECOVER,让轴能自行归位
|
||||
* (烧录/上电会短暂失力矩,轴会掉到限位,必须能自己爬起来);
|
||||
* - 但限位外若连续 GIMBAL_PITCH_LIMIT_STALL_MS 编码器一动不动,判为堵转/
|
||||
* 撞死,允许电流降到 GIMBAL_PITCH_LIMIT_CURRENT,避免长时间过载发热。
|
||||
* 轴回到软限位内后闭环自动恢复正常输出。 */
|
||||
#define GIMBAL_PITCH_LIMIT_CURRENT_RECOVER 12000.0f // 限位外抬升方向允许电流(≈2.2A,额定3A的73%)
|
||||
#define GIMBAL_PITCH_LIMIT_CURRENT 1500.0f // 限位外判定堵转后的涓流上限(≈0.27A)
|
||||
#define GIMBAL_PITCH_LIMIT_STALL_MS 300 // 限位外不动多久判定堵转(高位电流只持续这么久)
|
||||
#endif
|
||||
/* ================= 以上为停用块,以下为原有代码 ================= */
|
||||
|
||||
/* 1=把 CAN 掉线警告音替换成循环播放的旋律《你》(RM动画片尾曲),报警时播放;
|
||||
* 旋律音符表见 modules/dev/buzzer.c 的 buzzer_melody[] */
|
||||
#define DEBUG_BUZZER_MELODY 0
|
||||
|
||||
Reference in New Issue
Block a user