153 lines
9.6 KiB
C
153 lines
9.6 KiB
C
/* =============================================================================
|
|
* robot_param.h - 全部个性化参数(整车唯一调参入口)
|
|
* 分区:云台限位/转速 -> 云台 Yaw/Pitch PID(双陀螺环) -> 摩擦轮/拨弹 ->
|
|
* 底盘几何/轮速 PID/跟随(双 INS 世界系)/功率 -> CToC 帧 ID ->
|
|
* CAN ID 表 -> 掉线时间 -> IMU 恒温 -> 台架调试开关
|
|
* 控制周期 2ms(改周期需重调所有 PID!)
|
|
* ========================================================================== */
|
|
#ifndef ROBOT_PARAM_H
|
|
#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 4440 // Yaw 轴编码器零位(机械正前方,0~8191)
|
|
#define CHASSIS_SPIN_FEEDFORWARD_K 0.0725f // 小陀螺解算时延前馈补偿系数(s)
|
|
|
|
#define GIMBAL_FRICTION_WHEEL_SPEED 5500.0f // 摩擦轮转速(RPM)
|
|
#define GIMBAL_RAMMER_SPEED -5400.0f// 拨弹盘转速(RPM,负向发射)
|
|
|
|
#define GIMBAL_LEVER_SPEED_MAP_RATE 2.0f // 云台拨杆速度映射比例(Yaw 用)
|
|
#define GIMBAL_PITCH_LEVER_RATE 5.0f // Pitch 专用比例(满杆≈110°/s)
|
|
#define GIMBAL_YAW_PITCH_SPEED_RATE 3.0f // 云台偏航/俯仰速度比
|
|
#define PC_MOUSE_RL_SENSITIVITY 3.0f // 鼠标左右灵敏度
|
|
#define PC_MOUSE_DU_SENSITIVITY 8.0f // 鼠标上下灵敏度
|
|
|
|
/* ---- Yaw 双环(2ms):位置环与速度环都以陀螺仪为反馈 ----
|
|
* 设计要点(2026-09 重写):
|
|
* - 旧方案速度环用电机转子转速、位置环用陀螺角度,两类传感器的方向关系
|
|
* 由装配(皮带是否交叉)决定,一个符号参数无法同时保证两环负反馈——
|
|
* 这是反复"疯转"的根源,已废弃;
|
|
* - 现在两环共用陀螺,整条链只有一个装配符号,由上电自检自动测定
|
|
* (GIMBAL_YAW_DETECT 见下),无需人工猜;
|
|
* - 单位:角度环 误差° -> 目标角速度°/s;速度环 误差°/s -> 电压。
|
|
* 调参顺序:先速度环(点动不振荡),再位置环(刚性)——见移植 README §8.1 */
|
|
#define GIMBAL_YAW_ANGLE_KP 20.0f // ° -> °/s:10° 误差给 150°/s
|
|
#define GIMBAL_YAW_ANGLE_KI 0.0f
|
|
#define GIMBAL_YAW_ANGLE_KD 0.0f // 速度环本身即微分,不再叠加
|
|
#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_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)
|
|
|
|
/* 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_RATE_MIN 20.0f // 判定转动的陀螺速率阈值(°/s)
|
|
|
|
/* ---- Pitch(位置环陀螺 Roll,速度环转子转速——Pitch 无符号史问题,维持原案) ---- */
|
|
#define GIMBAL_PITCH_ANGLE_EK_DEADBAND 0.3f // 位置环误差死区(°)
|
|
#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_FRICTION_WHEEL_KP 16.0f
|
|
#define GIMBAL_FRICTION_WHEEL_KI 0.0f
|
|
#define GIMBAL_FRICTION_WHEEL_KD 30.0f
|
|
|
|
#define GIMBAL_RAMMER_KP 16.0f
|
|
#define GIMBAL_RAMMER_KI 0.0f
|
|
#define GIMBAL_RAMMER_KD 0.0f
|
|
|
|
/* ============================ 底盘 ============================ */
|
|
#define MECANUM_WHEEL_RADIUS 7.0f // 麦轮半径(cm)
|
|
#define MECANUM_RX 18.0f // 底盘中心到轮心的x轴分量(cm)
|
|
#define MECANUM_RY 18.75f // 底盘中心到轮心的y轴分量(cm)
|
|
/* 转速换算系数 = 19(减速比)*60*100/(2π*R_cm) */
|
|
#define MECANUM_SPEED_FACTOR 18143.663512f
|
|
|
|
#define MECANUM_CHASSIS_SPEED_SCALE 0.004f // 摇杆->线速度比例(legacy)
|
|
#define MECANUM_KEYBOARD_SPEED 2.4f // 键盘 WASD 平移速度(m/s 量纲映射值)
|
|
#define MECANUM_SPIN_SLOW_W 4.0f // 慢速小陀螺角速度(rad/s 量纲映射值)
|
|
#define MECANUM_SPIN_FAST_W 7.0f // 快速小陀螺角速度(legacy 7.0)
|
|
#define MECANUM_SHIFT_BOOST_RATE 1.3f // SHIFT 加速增益(超电接入前的临时方案)
|
|
|
|
/* 底盘轮速 PID(legacy 已调参数,2ms 控制周期) */
|
|
#define MECANUM_WHEEL_SPEED_KP 0.8f
|
|
#define MECANUM_WHEEL_SPEED_KI 0.0f
|
|
#define MECANUM_WHEEL_SPEED_KD 0.0f
|
|
#define MECANUM_WHEEL_SPEED_OUT_LIMIT 15000.0f
|
|
|
|
/* ---- 跟随(2ms):双 INS 世界系方案(2026-09 重写) ----
|
|
* 云台把世界系 yaw 经 0x1A9 帧发给底盘,底盘用自己的 INS yaw 相减:
|
|
* err = wrap180(云台yaw - 底盘yaw)
|
|
* 不再依赖底盘侧 GM6020(0x205) 反馈,皮带虚位不在环内,无基准锁定时序坑。
|
|
* 两板 INS 各自上电零点任意,但差值只随相对转动变化,天然正确 */
|
|
#define CHASSIS_FOLLOW_KP 0.4678f // w 量纲/°:10° 偏差给 w=4
|
|
#define CHASSIS_FOLLOW_KI 0.0f // 调试期 0,同旧教训(积分残留)
|
|
#define CHASSIS_FOLLOW_KD 24.452f
|
|
#define CHASSIS_FOLLOW_OUT_LIMIT 4.5f // 输出限幅(介于慢/快陀螺之间)
|
|
#define CHASSIS_FOLLOW_DEADBAND 1.0f // 误差死区(°)
|
|
|
|
/* 跟随符号全自动综合:云台自检(0x1A9 flags)下发编码器 tick 增向,
|
|
* 底盘自检用小 w 脉冲+INS 测 w 正向对应的车身转向,两者相乘。
|
|
* CHASSIS_FOLLOW_SIGN 仅作自检失败时的回退值 */
|
|
#define CHASSIS_FOLLOW_SIGN (1.0f)
|
|
#define CHASSIS_DETECT_W 1.2f // 自检 w 脉冲幅值
|
|
#define CHASSIS_DETECT_TIME_MS 80 // 脉冲时长(落地整车轻转)
|
|
#define CHASSIS_DETECT_YAW_MIN 0.5f // 判定转动的 INS yaw 阈值(°)
|
|
|
|
/* 功率限制(简易方案:缓冲能量不足时按比例缩放轮电流) */
|
|
#define POWER_LIMIT_BUFFER_LOW 30.0f // 缓冲能量低于此值开始限流(J)
|
|
#define POWER_LIMIT_BUFFER_MIN 10.0f // 缓冲能量最低保护值(J)
|
|
|
|
/* ============================ CToC 板间通讯 ============================ */
|
|
#define CTOC_MASTER_ID1 0x019 // 底盘->云台:发射机构状态+底盘故障位
|
|
#define CTOC_SLAVE_ID1 0x149 // 云台->底盘:摇杆数据
|
|
#define CTOC_SLAVE_ID2 0x189 // 云台->底盘:控制状态+故障/陀螺使能位
|
|
#define CTOC_SLAVE_ID3 0x1A9 // 云台->底盘:云台世界系 yaw(float)+att_ready
|
|
|
|
/* ============================ CAN ID ============================ */
|
|
/* 云台板:CAN1 = Pitch GM6020(0x206)+摩擦轮 M3508(0x201/0x202)
|
|
* CAN2 = Yaw GM6020(0x205)+底盘回传(0x019)
|
|
* 底盘板:CAN1 = 四轮 M3508(0x201~0x204)
|
|
* CAN2 = 云台数据(0x149/0x189/0x1A9) */
|
|
|
|
/* ============================ 台架调试开关 ============================ */
|
|
#define DEBUG_SHOOT_WITHOUT_REFEREE 1 // 1=无裁判系统时也允许开摩擦轮(台架调试用),比赛请改 0
|
|
#define GIMBAL_RAMMER_INSTALLED 0 // 1=拨弹 M2006 已安装;0=暂缺,不参与掉线判定
|
|
|
|
/* 1=调试模式:把 CAN 掉线警告音替换成循环播放的旋律(台架调试时避免连续滴滴声
|
|
* 干扰)。红灯常亮与其它灯语完全不变,仍能看出是哪台设备掉线;比赛请改 0。
|
|
* 旋律音符表见 modules/BUZZER/buzzer.c 的 buzzer_melody[] */
|
|
#define DEBUG_BUZZER_MELODY 1
|
|
#define BUZZER_MELODY_SPEED_DIV 1 // 旋律降速分频(默认1:每步100ms精准匹配原版带吐音断音节拍) // 旋律降速分频(默认2:放慢一倍匹配原曲抒情节拍;设为3/4可进一步放慢)
|
|
|
|
/* ============================ 掉线检测 ============================ */
|
|
#define DEVICE_OFFLINE_TIME_MS 100 // CAN 设备/裁判系统掉线判定时间
|
|
#define CTOC_OFFLINE_TIME_MS 100 // 板间通讯掉线判定时间
|
|
|
|
/* ============================ IMU ============================ */
|
|
/* 姿态解算为 QuaternionEKF(见 modules/ins);z 轴零偏上电静止标定约 1s,
|
|
* x/y 由 EKF 在线估计;EKF 参数(Q1/Q2/R/lambda)在 ins.c 的 ins_init() 内调整 */
|
|
|
|
#define IMU_TEMPERATURE_TARGET 40.0f // BMI088 恒温目标(℃)
|
|
#define IMU_TEMPERATURE_LIMIT 50.0f // 温度报警阈值(℃)
|
|
|
|
#define IMU_TEMP_KP 20.0f
|
|
#define IMU_TEMP_KI 0.0003f
|
|
#define IMU_TEMP_KD 0.0f
|
|
|
|
#endif
|