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) */
底盘/modules/motor/dji_motor.c
+34 -4
View File
@@ -6,9 +6,9 @@
* (无 legacy 的 +-1 圈限制,底盘小陀螺时 Yaw 可连续旋转),
* 记录 last_online_tick 供掉线检测
* 控制路径(任务上下文):
* dji_motor_send_current_low/high() 0x200 / 0x1FF 电流帧(+/-16384 = 20A/10A)
* dji_motor_send_voltage_high() 0x2FF 电压帧(GM6020 ID5~7)
* 注意 0x1FF 帧为 M3508/M2006 高位电流与 GM6020 低位电压共用,按槽位区分
* dji_motor_send_current_low/high() 0x200 / 0x1FF 电流帧(+/-16384 = 20A/10A)
* dji_motor_send_gm6020_current_low/high() 0x1FE / 0x2FE 电流帧(GM6020 +/-16384 = 3A)
* dji_motor_send_voltage_high() 0x2FF 电压帧(GM6020 旧协议)
* ========================================================================== */
#include "dji_motor.h"
@@ -53,7 +53,7 @@ static void dji_motor_parse(dji_motor_t * motor, const uint8_t data[8])
fdb->torque_current = fdb->raw_torque * 0.001f; // /10000*10
break;
case DJI_MOTOR_6020:
fdb->torque_current = fdb->raw_torque * -3.0f; // 实际转矩电流,±3A 量程
fdb->torque_current = (float)fdb->raw_torque * (3.0f / 16384.0f); // 实际转矩电流,±3A 量程
break;
}
fdb->last_online_tick = HAL_GetTick();
@@ -107,6 +107,13 @@ void dji_motor_send_int16x4(hcan_t * hcan, uint32_t std_id, const int16_t val[4]
CAN_SendTxMessage(&msg);
}
static inline int16_t dji_motor_clamp16(int16_t val, int16_t min_val, int16_t max_val)
{
if (val > max_val) return max_val;
if (val < min_val) return min_val;
return val;
}
void dji_motor_send_current_low(hcan_t * hcan, int16_t i1, int16_t i2, int16_t i3, int16_t i4)
{
const int16_t v[4] = {i1, i2, i3, i4};
@@ -119,8 +126,31 @@ void dji_motor_send_current_high(hcan_t * hcan, int16_t i5, int16_t i6, int16_t
dji_motor_send_int16x4(hcan, 0x1FF, v);
}
void dji_motor_send_gm6020_current_low(hcan_t * hcan, int16_t i1, int16_t i2, int16_t i3, int16_t i4)
{
const int16_t v[4] = {
dji_motor_clamp16(i1, -16384, 16384),
dji_motor_clamp16(i2, -16384, 16384),
dji_motor_clamp16(i3, -16384, 16384),
dji_motor_clamp16(i4, -16384, 16384)
};
dji_motor_send_int16x4(hcan, 0x1FE, v);
}
void dji_motor_send_gm6020_current_high(hcan_t * hcan, int16_t i5, int16_t i6, int16_t i7)
{
const int16_t v[4] = {
dji_motor_clamp16(i5, -16384, 16384),
dji_motor_clamp16(i6, -16384, 16384),
dji_motor_clamp16(i7, -16384, 16384),
0
};
dji_motor_send_int16x4(hcan, 0x2FE, v);
}
void dji_motor_send_voltage_high(hcan_t * hcan, int16_t v5, int16_t v6, int16_t v7)
{
const int16_t v[4] = {v5, v6, v7, 0};
dji_motor_send_int16x4(hcan, 0x2FF, v);
}
底盘/modules/motor/dji_motor.h
+9 -2
View File
@@ -39,10 +39,17 @@ extern void dji_motor_init(dji_motor_t * motor, hcan_t * hcan, uint16_t rx_id, d
/* ---- 控制帧发送 ----
* M3508/M2006 ID1~4:0x200 电流帧(±16384 对应 ±20A/±10A)
* M3508/M2006 ID5~8:0x1FF 电流帧;GM6020 ID1~4:0x1FF 电压帧(±25000)
* GM6020 ID5~7:0x2FF 电压帧 */
* M3508/M2006 ID5~8:0x1FF 电流帧(±16384 对应 ±20A/±10A)
* GM6020 电流控制模式(±16384 对应 ±3A):
* GM6020 ID1~4:0x1FE 电流帧
* GM6020 ID5~7:0x2FE 电流帧
* GM6020 旧电压控制模式(±25000 对应 ±24V):
* GM6020 ID1~4:0x1FF 电压帧
* GM6020 ID5~7:0x2FF 电压帧 */
extern void dji_motor_send_current_low(hcan_t * hcan, int16_t i1, int16_t i2, int16_t i3, int16_t i4);
extern void dji_motor_send_current_high(hcan_t * hcan, int16_t i5, int16_t i6, int16_t i7, int16_t i8);
extern void dji_motor_send_gm6020_current_low(hcan_t * hcan, int16_t i1, int16_t i2, int16_t i3, int16_t i4);
extern void dji_motor_send_gm6020_current_high(hcan_t * hcan, int16_t i5, int16_t i6, int16_t i7);
extern void dji_motor_send_voltage_high(hcan_t * hcan, int16_t v5, int16_t v6, int16_t v7);
/* 通用 int16x4 发送(板间通讯等复用) */