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/Task/Inc/app_tasks.h
+1
View File
@@ -46,5 +46,6 @@ extern void chassis_task(void const * argument); // 仅底盘板
extern void c_comm_task(void const * argument);
extern void referee_task(void const * argument);
extern void debug_task(void const * argument); // USB 串口诊断输出
extern void shoot_detect_task(void const * argument); // 摩擦轮发射波动检测任务
#endif
云台/Application/Task/Inc/shoot_detect_task.h
+159
View File
@@ -0,0 +1,159 @@
/* =============================================================================
* shoot_detect_task.h - 摩擦轮电流/电压波动发射检测
*
* 检测原理:
* 当弹丸被挤压通过两高速旋转的摩擦轮(M3508)瞬间, 机械负荷剧增,
* 导致左右摩擦轮电机的转矩电流/控制输出在 5~15ms 内产生极其显著的脉冲尖峰。
* 算法利用自适应基线(一阶低通滤波)估计空载电流, 实时计算瞬时突变量:
* ΔI = |I_current| - I_baseline
* 当 ΔI 超过突变阈值, 且摩擦轮处于正常高转速状态时, 触发单发判定。
* 引入防抖不应期(Cooldown), 杜绝单发弹丸通过过程中的重复计数。
* ========================================================================== */
#ifndef SHOOT_DETECT_TASK_H
#define SHOOT_DETECT_TASK_H
#include <stdint.h>
#include <stdbool.h>
#ifdef __cplusplus
extern "C" {
#endif
/* ============================ 默认参数配置 ============================ */
/* 电流突变触发阈值(A):
* M3508 空载稳态电流通常在 0.5A~1.5A 左右, 弹丸挤压通过时瞬时电流常达 3.5A~7A+。
* 建议突变量阈值设为 1.8A ~ 2.5A, 既能灵敏捕捉发弹, 又能避开空载抖动。 */
#ifndef SHOOT_DETECT_CURRENT_THRESHOLD_A
#define SHOOT_DETECT_CURRENT_THRESHOLD_A 2.0f
#endif
/* 摩擦轮启动加速稳定等待时间(ms):
* 开启摩擦轮后等待电机从 0 加速到 5500 RPM 并平稳进入空载稳态的时间,
* 期间处于 SPINUP 状态, 屏蔽加速满幅电流, 严防误触发 */
#ifndef SHOOT_DETECT_SPINUP_TIMEOUT_MS
#define SHOOT_DETECT_SPINUP_TIMEOUT_MS 500
#endif
/* 摩擦轮有效工作最低转速(RPM): 默认 4500 RPM(接近 5500 稳态转速) */
#ifndef SHOOT_DETECT_SPEED_MIN_RPM
#define SHOOT_DETECT_SPEED_MIN_RPM 4500.0f
#endif
/* 发射防抖不应期(ms):
* 单发弹丸通过摩擦轮通常持续 5~15ms, 设为 35ms 可确保同一发只计一次,
* 同时支持最高 25发/秒 的高射频(周期 40ms) */
#ifndef SHOOT_DETECT_COOLDOWN_MS
#define SHOOT_DETECT_COOLDOWN_MS 35
#endif
/* 空载基线平滑系数(0.0 ~ 1.0): 用于自适应跟踪温度和磨损带来的稳态电流漂移 */
#ifndef SHOOT_DETECT_BASELINE_ALPHA
#define SHOOT_DETECT_BASELINE_ALPHA 0.05f
#endif
/* ============================ 状态枚举 ============================ */
typedef enum
{
SHOOT_DETECT_STATE_STOPPED = 0, // 摩擦轮未启动: 检测器休眠/禁用, 不进行任何检测
SHOOT_DETECT_STATE_SPINUP, // 摩擦轮起转加速中: 等待转速达到工作点并稳定
SHOOT_DETECT_STATE_ACTIVE, // 摩擦轮已启动且稳态运行: 发射检测正式就绪激活
} shoot_detect_state_e;
/* ============================ 数据结构 ============================ */
typedef struct
{
/* 运行状态 */
shoot_detect_state_e state; // 当前状态(STOPPED / SPINUP / ACTIVE)
uint8_t is_ready; // 摩擦轮是否已完全启动并就绪(1=ACTIVE)
uint32_t spinup_start_tick; // 摩擦轮启动时刻(HAL_GetTick)
/* 发射统计 */
uint32_t shoot_count; // 累计发射弹丸总数
volatile uint8_t fired_pulse; // 单发脉冲标志(读取后自动清除或通过 API 清除)
float shoot_freq_hz; // 瞬时估算射频(Hz)
uint32_t last_shoot_tick; // 上次发射时间戳(HAL_GetTick)
/* 瞬时信号与基线(A) */
float current_l; // 左摩擦轮当前绝对电流
float current_r; // 右摩擦轮当前绝对电流
float baseline_l; // 左摩擦轮空载基线电流
float baseline_r; // 右摩擦轮空载基线电流
float delta_l; // 左摩擦轮电流突变量
float delta_r; // 右摩擦轮电流突变量
float delta_max; // 左右轮最大突变量
/* 配置阈值 */
float threshold_a; // 触发阈值(A)
uint32_t cooldown_ms; // 防抖不应期(ms)
float speed_min_rpm; // 最低有效转速(RPM)
uint32_t spinup_timeout_ms; // 起转稳定时间(ms)
} shoot_detect_t;
/* ============================ 接口函数 ============================ */
/**
* @brief 初始化发射检测模块
*/
void shoot_detect_init(void);
/**
* @brief 发射检测核心周期更新函数(推荐在 2ms shoot_task 中调用)
*/
void shoot_detect_update(void);
/**
* @brief 查询摩擦轮是否已启动并处于就绪检测状态
* @retval 1: 摩擦轮已启动且稳态运行, 检测器就绪
* 0: 摩擦轮未启动或正在加速启动中
*/
uint8_t shoot_detect_is_ready(void);
/**
* @brief 获取当前检测器状态(STOPPED/SPINUP/ACTIVE)
*/
shoot_detect_state_e shoot_detect_get_state(void);
/**
* @brief 查询是否有新的一发弹丸发射(单发脉冲)
* @retval 1: 检测到新发射(读取后内部自动清除标志)
* 0: 无新发射
*/
uint8_t shoot_detect_has_fired(void);
/**
* @brief 获取累计发射总发数
* @retval 累计发射数量
*/
uint32_t shoot_detect_get_count(void);
/**
* @brief 清零发射总发数计数器
*/
void shoot_detect_clear_count(void);
/**
* @brief 获取当前瞬时射频(Hz)
* @retval 射频
*/
float shoot_detect_get_freq(void);
/**
* @brief 获取完整检测器状态结构体指针(供调试和上位机打印)
*/
const shoot_detect_t * shoot_detect_get_data(void);
/**
* @brief 动态设置检测电流阈值(A)
*/
void shoot_detect_set_threshold(float threshold_a);
/**
* @brief 可选独立 FreeRTOS 任务实现(若用户希望作为单独任务运行)
*/
void shoot_detect_task(void const * argument);
#ifdef __cplusplus
}
#endif
#endif /* SHOOT_DETECT_TASK_H */
云台/Application/Task/Src/shoot_detect_task.c
+239
View File
@@ -0,0 +1,239 @@
/* =============================================================================
* shoot_detect_task.c - 摩擦轮电流/电压波动发射检测实现
* 前提条件: 摩擦轮开启且加速稳定后(STATE_ACTIVE)才进行发射检测判定
* ========================================================================== */
#include "shoot_detect_task.h"
#include "gimbal.h"
#include "shoot.h"
#include "cmsis_os.h"
#include "FreeRTOS.h"
#include "task.h"
#include <math.h>
static shoot_detect_t g_shoot_detect;
void shoot_detect_init(void)
{
g_shoot_detect.state = SHOOT_DETECT_STATE_STOPPED;
g_shoot_detect.is_ready = 0;
g_shoot_detect.spinup_start_tick = 0;
g_shoot_detect.shoot_count = 0;
g_shoot_detect.fired_pulse = 0;
g_shoot_detect.shoot_freq_hz = 0.0f;
g_shoot_detect.last_shoot_tick = 0;
g_shoot_detect.current_l = 0.0f;
g_shoot_detect.current_r = 0.0f;
g_shoot_detect.baseline_l = 0.0f;
g_shoot_detect.baseline_r = 0.0f;
g_shoot_detect.delta_l = 0.0f;
g_shoot_detect.delta_r = 0.0f;
g_shoot_detect.delta_max = 0.0f;
g_shoot_detect.threshold_a = SHOOT_DETECT_CURRENT_THRESHOLD_A;
g_shoot_detect.cooldown_ms = SHOOT_DETECT_COOLDOWN_MS;
g_shoot_detect.speed_min_rpm = SHOOT_DETECT_SPEED_MIN_RPM;
g_shoot_detect.spinup_timeout_ms = SHOOT_DETECT_SPINUP_TIMEOUT_MS;
}
void shoot_detect_update(void)
{
uint32_t now = HAL_GetTick();
/* 检查摩擦轮电机在线状态 */
if (motor_friction_l.fdb.last_online_tick == 0 ||
motor_friction_r.fdb.last_online_tick == 0 ||
(now - motor_friction_l.fdb.last_online_tick > 100) ||
(now - motor_friction_r.fdb.last_online_tick > 100))
{
g_shoot_detect.state = SHOOT_DETECT_STATE_STOPPED;
g_shoot_detect.is_ready = 0;
return;
}
/* 核心前提: 摩擦轮必须处于启动开启状态 (shoot_friction_flag == 1) */
if (!shoot_friction_flag)
{
/* 摩擦轮未启动: 强制处于 STOPPED 状态, 复位所有检测信号 */
g_shoot_detect.state = SHOOT_DETECT_STATE_STOPPED;
g_shoot_detect.is_ready = 0;
g_shoot_detect.current_l = 0.0f;
g_shoot_detect.current_r = 0.0f;
g_shoot_detect.delta_l = 0.0f;
g_shoot_detect.delta_r = 0.0f;
g_shoot_detect.delta_max = 0.0f;
g_shoot_detect.baseline_l = 0.0f;
g_shoot_detect.baseline_r = 0.0f;
return;
}
/* 读取当前两轮绝对电流(A)与实际转速(RPM) */
float cur_l = fabsf(motor_friction_l.fdb.torque_current);
float cur_r = fabsf(motor_friction_r.fdb.torque_current);
float spd_l = fabsf((float)motor_friction_l.fdb.rotor_speed);
float spd_r = fabsf((float)motor_friction_r.fdb.rotor_speed);
g_shoot_detect.current_l = cur_l;
g_shoot_detect.current_r = cur_r;
/* ---------------- 状态机流转 ---------------- */
switch (g_shoot_detect.state)
{
case SHOOT_DETECT_STATE_STOPPED:
/* 摩擦轮刚刚开启, 记录起转时刻并进入起转加速过渡期 */
g_shoot_detect.spinup_start_tick = now;
g_shoot_detect.state = SHOOT_DETECT_STATE_SPINUP;
g_shoot_detect.is_ready = 0;
break;
case SHOOT_DETECT_STATE_SPINUP:
/* 加速过渡期: 屏蔽电机刚启动加速时的满幅大电流,
* 等待起转时间满足(默认 500ms) 且 双轮转速均达到额定工作点(默认 4500 RPM) */
if ((now - g_shoot_detect.spinup_start_tick >= g_shoot_detect.spinup_timeout_ms) &&
(spd_l >= g_shoot_detect.speed_min_rpm) &&
(spd_r >= g_shoot_detect.speed_min_rpm))
{
/* 捕获稳态旋转初始基线, 正式就绪激活检测 */
g_shoot_detect.baseline_l = cur_l;
g_shoot_detect.baseline_r = cur_r;
g_shoot_detect.state = SHOOT_DETECT_STATE_ACTIVE;
g_shoot_detect.is_ready = 1;
}
break;
case SHOOT_DETECT_STATE_ACTIVE:
/* 正常工作检测期: 检查转速是否异常跌落过低(如严重堵转或掉电) */
if (spd_l < (g_shoot_detect.speed_min_rpm - 1000.0f) ||
spd_r < (g_shoot_detect.speed_min_rpm - 1000.0f))
{
/* 转速异常跌落, 退回起转过渡等待重新稳定 */
g_shoot_detect.spinup_start_tick = now;
g_shoot_detect.state = SHOOT_DETECT_STATE_SPINUP;
g_shoot_detect.is_ready = 0;
break;
}
/* 计算两轮当前电流相对于空载基线的突变量 ΔI */
g_shoot_detect.delta_l = cur_l - g_shoot_detect.baseline_l;
g_shoot_detect.delta_r = cur_r - g_shoot_detect.baseline_r;
float delta_l_valid = (g_shoot_detect.delta_l > 0.0f) ? g_shoot_detect.delta_l : 0.0f;
float delta_r_valid = (g_shoot_detect.delta_r > 0.0f) ? g_shoot_detect.delta_r : 0.0f;
g_shoot_detect.delta_max = (delta_l_valid > delta_r_valid) ? delta_l_valid : delta_r_valid;
/* 自适应基线平滑更新:
* 仅在电流未出现大波动(处于空载稳态区间)时平滑更新基线,
* 避免弹丸过轮脉冲把基线强行拉高 */
if (g_shoot_detect.delta_max < g_shoot_detect.threshold_a * 0.6f)
{
g_shoot_detect.baseline_l += SHOOT_DETECT_BASELINE_ALPHA * (cur_l - g_shoot_detect.baseline_l);
g_shoot_detect.baseline_r += SHOOT_DETECT_BASELINE_ALPHA * (cur_r - g_shoot_detect.baseline_r);
}
/* 发射事件波峰判定 */
if (g_shoot_detect.delta_max >= g_shoot_detect.threshold_a)
{
/* 防抖不应期(Cooldown): 单发过轮持续 5~15ms,
* 冷却时间内不重复计数, 确保单发只计一次 */
if (now - g_shoot_detect.last_shoot_tick >= g_shoot_detect.cooldown_ms)
{
uint32_t dt = now - g_shoot_detect.last_shoot_tick;
if (dt > 0 && dt <= 1000)
{
g_shoot_detect.shoot_freq_hz = 1000.0f / (float)dt;
}
else if (dt > 1000)
{
g_shoot_detect.shoot_freq_hz = 1.0f;
}
g_shoot_detect.last_shoot_tick = now;
g_shoot_detect.shoot_count++;
g_shoot_detect.fired_pulse = 1;
}
}
break;
}
}
uint8_t shoot_detect_is_ready(void)
{
return g_shoot_detect.is_ready;
}
shoot_detect_state_e shoot_detect_get_state(void)
{
return g_shoot_detect.state;
}
uint8_t shoot_detect_has_fired(void)
{
if (g_shoot_detect.fired_pulse)
{
g_shoot_detect.fired_pulse = 0;
return 1;
}
return 0;
}
uint32_t shoot_detect_get_count(void)
{
return g_shoot_detect.shoot_count;
}
void shoot_detect_clear_count(void)
{
g_shoot_detect.shoot_count = 0;
}
float shoot_detect_get_freq(void)
{
uint32_t now = HAL_GetTick();
/* 若超过 1s 无新发射, 瞬时射频归零 */
if (now - g_shoot_detect.last_shoot_tick > 1000)
{
g_shoot_detect.shoot_freq_hz = 0.0f;
}
return g_shoot_detect.shoot_freq_hz;
}
const shoot_detect_t * shoot_detect_get_data(void)
{
return &g_shoot_detect;
}
void shoot_detect_set_threshold(float threshold_a)
{
if (threshold_a > 0.1f)
{
g_shoot_detect.threshold_a = threshold_a;
}
}
void shoot_detect_task(void const * argument)
{
(void)argument;
shoot_detect_init();
extern volatile uint8_t app_ready;
while (!app_ready)
osDelay(2);
TickType_t last_wake = xTaskGetTickCount();
for (;;)
{
/* 前提: 若摩擦轮未开启, 低频 10ms 休眠等待, 不占用密集 CPU 计算 */
if (!shoot_friction_flag)
{
shoot_detect_update();
osDelay(10);
last_wake = xTaskGetTickCount();
continue;
}
/* 摩擦轮开启中, 以 2ms 高精度周期进行波动检测 */
shoot_detect_update();
vTaskDelayUntil(&last_wake, pdMS_TO_TICKS(2));
}
}
云台/Application/Task/Src/shoot_task.c
+4
View File
@@ -12,6 +12,7 @@
#include "shoot.h"
#include "c_comm.h"
#include "dr16.h"
#include "shoot_detect_task.h"
/* shoot_task(云台板,2ms):摩擦轮/拨弹控制
* 发射机构上电状态来自底盘 0x019(ctoc_shooter_power) */
@@ -20,6 +21,8 @@ void shoot_task(void const * argument)
{
TickType_t last_wake = xTaskGetTickCount();
shoot_detect_init();
while (!app_ready)
osDelay(2);
@@ -30,6 +33,7 @@ void shoot_task(void const * argument)
shoot_control(cmd_robot.friction_on, cmd_robot.fire, cmd_robot.eject,
ctoc_shooter_power);
shoot_send_cmd();
shoot_detect_update();
}
else
{
云台/Application/robot_param.h
+26 -13
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 ============================ */
#define GIMBAL_RAMMER_CAN_ID 0x201 // 拨弹盘 M2006 反馈 ID(CAN2 0x200 控制帧槽 1, 对应电机拨码 ID 1)
/* 云台板:CAN1 = Pitch GM6020(0x206) + 摩擦轮 M3508(0x201/0x202)
* CAN2 = Yaw GM6020(0x205)+底盘回传(0x019)
* CAN2 = Yaw GM6020(0x205) + 拨弹 M2006(0x201) + 底盘回传(0x019)
* 底盘板:CAN1 = 四轮 M3508(0x201~0x204)
* CAN2 = 云台数据(0x149/0x189/0x1A9) */
云台/modules/control/gimbal.c
+40 -31
View File
@@ -1,12 +1,12 @@
/* =============================================================================
* gimbal.c - 云台双轴控制(串级 PID)
* gimbal.c - 云台双轴控制(串级 PID, 全电流环模式)
* Yaw(2026-09 重写):双陀螺环,见 gimbal.h 与 robot_param.h 说明
* 位置环:err(°) = 目标角 - SIGN*att_deg_yaw -> 目标角速度(°/s,限幅)
* 速度环:err(°/s) = 目标角速度 - SIGN*gyro_z -> 电压(限幅)
* 稳定性条件:SIGN*G>0,G 为"电压->陀螺 z 角速度"的装配增益,
* 速度环:err(°/s) = 目标角速度 - SIGN*gyro_z -> 电流(限幅 ±16384)
* 稳定性条件:SIGN*G>0,G 为"电流->陀螺 z 角速度"的装配增益,
* 由 gimbal_detect_yaw_sign() 上电实测,彻底消除双传感器符号打架
* Pitch:位置环用 QEKF Roll(安装方向),速度环转子转速(legacy 案,无符号史问题)
* 发送:CAN2 0x1FF 槽1=Yaw 电压;CAN1 0x1FF 槽2=Pitch 电压,槽3=拨弹电流
* Pitch(电流环重整):位置环陀螺 Roll -> 期望转速(RPM), 速度环转子转速 -> 电流(限幅 ±16384)
* 发送:CAN2 0x1FE 槽1=Yaw 电流; CAN1 0x1FE 槽2=Pitch 电流; CAN1 0x1FF 槽3=拨弹电流
* ========================================================================== */
#include "gimbal.h"
#include "shoot.h"
@@ -34,8 +34,8 @@ static PID_PositionTypeDef pitch_angle_pid, pitch_speed_pid;
static float yaw_target_deg; // 世界系目标角(°)
static float pitch_target_deg; // 目标角(°)
static float yaw_voltage;
static float pitch_voltage;
static float yaw_current;
static float pitch_current;
/* 装配符号:0=未检测,±1=已测定;未检测前 gimbal_control 不应被调用 */
static float yaw_sign_f = 0.0f;
@@ -52,7 +52,7 @@ void gimbal_motor_init(void)
dji_motor_init(&motor_pitch_gimbal, &hcan1, 0x206, DJI_MOTOR_6020);
dji_motor_init(&motor_friction_l, &hcan1, 0x201, DJI_MOTOR_3508);
dji_motor_init(&motor_friction_r, &hcan1, 0x202, DJI_MOTOR_3508);
dji_motor_init(&motor_rammer, &hcan1, 0x207, DJI_MOTOR_2006);
dji_motor_init(&motor_rammer, &hcan2, GIMBAL_RAMMER_CAN_ID, DJI_MOTOR_2006);
/* Yaw 位置环:° -> °/s */
PID_PositionStructureInit(&yaw_angle_pid, 0);
@@ -60,21 +60,21 @@ void gimbal_motor_init(void)
PID_PositionSetEkRange(&yaw_angle_pid, -GIMBAL_YAW_ANGLE_EK_DEADBAND, GIMBAL_YAW_ANGLE_EK_DEADBAND);
PID_PositionSetOUTRange(&yaw_angle_pid, -GIMBAL_YAW_ANGLE_OUT_LIMIT, GIMBAL_YAW_ANGLE_OUT_LIMIT);
/* Yaw 速度环:°/s -> 电压 */
/* Yaw 速度环:°/s -> 电流 */
PID_PositionStructureInit(&yaw_speed_pid, 0);
PID_PositionSetParameter(&yaw_speed_pid, GIMBAL_YAW_SPEED_KP, GIMBAL_YAW_SPEED_KI, GIMBAL_YAW_SPEED_KD);
PID_PositionSetOUTRange(&yaw_speed_pid, -GIMBAL_YAW_SPEED_OUT_LIMIT, GIMBAL_YAW_SPEED_OUT_LIMIT);
/* Pitch:与 legacy 相同的两环(位置环反馈为 Roll) */
/* Pitch:位置环反馈为 Roll, 速度环反馈为电机转子转速 (GM6020 电流环) */
PID_PositionStructureInit(&pitch_angle_pid, 0);
PID_PositionSetParameter(&pitch_angle_pid, GIMBAL_PITCH_ANGLE_KP, GIMBAL_PITCH_ANGLE_KI, GIMBAL_PITCH_ANGLE_KD);
PID_PositionSetEkRange(&pitch_angle_pid, -GIMBAL_PITCH_ANGLE_EK_DEADBAND, GIMBAL_PITCH_ANGLE_EK_DEADBAND);
PID_PositionSetOUTRange(&pitch_angle_pid, -200, 200);
PID_PositionSetOUTRange(&pitch_angle_pid, -GIMBAL_PITCH_ANGLE_OUT_LIMIT, GIMBAL_PITCH_ANGLE_OUT_LIMIT);
PID_PositionStructureInit(&pitch_speed_pid, 300);
PID_PositionStructureInit(&pitch_speed_pid, 0);
PID_PositionSetParameter(&pitch_speed_pid, GIMBAL_PITCH_SPEED_KP, GIMBAL_PITCH_SPEED_KI, GIMBAL_PITCH_SPEED_KD);
PID_PositionSetEkRange(&pitch_speed_pid, -5, 5);
PID_PositionSetOUTRange(&pitch_speed_pid, -30000, 30000);
PID_PositionSetEkRange(&pitch_speed_pid, -GIMBAL_PITCH_SPEED_EK_DEADBAND, GIMBAL_PITCH_SPEED_EK_DEADBAND);
PID_PositionSetOUTRange(&pitch_speed_pid, -GIMBAL_PITCH_SPEED_OUT_LIMIT, GIMBAL_PITCH_SPEED_OUT_LIMIT);
}
uint8_t gimbal_detect_yaw_sign(void)
@@ -83,18 +83,18 @@ uint8_t gimbal_detect_yaw_sign(void)
return 0;
/* 静默期:输出清零等待机械/陀螺稳定 */
dji_motor_send_current_high(&hcan2, 0, 0, 0, 0);
dji_motor_send_gm6020_current_low(&hcan2, 0, 0, 0, 0);
HAL_Delay(50);
/* 正向电压脉冲 60ms,期间平均陀螺 z 角速度 */
/* 正向电流脉冲 60ms,期间平均陀螺 z 角速度 */
float sum = 0.0f;
dji_motor_send_current_high(&hcan2, GIMBAL_YAW_DETECT_VOLTAGE, 0, 0, 0);
dji_motor_send_gm6020_current_low(&hcan2, (int16_t)GIMBAL_YAW_DETECT_CURRENT, 0, 0, 0);
for (uint16_t i = 0; i < 60; i++)
{
sum += bmi088_gyro[2]; // rad/s(z 轴向上为正)
HAL_Delay(1);
}
dji_motor_send_current_high(&hcan2, 0, 0, 0, 0);
dji_motor_send_gm6020_current_low(&hcan2, 0, 0, 0, 0);
HAL_Delay(80); // 回稳,随后 gimbal_task 的首次控制会同步目标角
const float rate_dps = sum / 60.0f * RAD_TO_ANGLE;
@@ -169,7 +169,7 @@ void gimbal_control(float yaw_delta, float pitch_delta, uint8_t reset_flag)
yaw_angle_pid.Need_Value = yaw_target_deg;
PID_PositionCalc(&yaw_angle_pid, yaw_meas);
/* Yaw 速度环:°/s -> 电压(反馈为带符号陀螺角速度,双环符号自洽) */
/* Yaw 速度环:°/s -> 电流(反馈为带符号陀螺角速度,双环符号自洽) */
yaw_speed_pid.Need_Value = yaw_angle_pid.OUT;
PID_PositionCalc(&yaw_speed_pid, yaw_rate_meas);
@@ -179,22 +179,30 @@ void gimbal_control(float yaw_delta, float pitch_delta, uint8_t reset_flag)
{
spin_feedforward = GIMBAL_YAW_SPIN_FEEDFORWARD_K * (float)motor_yaw_gimbal.fdb.rotor_speed;
}
yaw_voltage = yaw_speed_pid.OUT + spin_feedforward;
if (yaw_voltage > GIMBAL_YAW_SPEED_OUT_LIMIT)
yaw_voltage = GIMBAL_YAW_SPEED_OUT_LIMIT;
else if (yaw_voltage < -GIMBAL_YAW_SPEED_OUT_LIMIT)
yaw_voltage = -GIMBAL_YAW_SPEED_OUT_LIMIT;
yaw_current = yaw_speed_pid.OUT + spin_feedforward;
if (yaw_current > GIMBAL_YAW_SPEED_OUT_LIMIT)
yaw_current = GIMBAL_YAW_SPEED_OUT_LIMIT;
else if (yaw_current < -GIMBAL_YAW_SPEED_OUT_LIMIT)
yaw_current = -GIMBAL_YAW_SPEED_OUT_LIMIT;
/* Pitch 串级闭环,位置环反馈为 AHRS Roll(legacy 语义) */
/* Pitch 串级闭环,位置环反馈为 AHRS Roll(对应 Pitch 空间姿态角) */
pitch_angle_pid.Need_Value = pitch_target_deg;
PID_PositionCalc(&pitch_angle_pid, att_deg_roll);
pitch_speed_pid.Need_Value = pitch_angle_pid.OUT;
PID_PositionCalc(&pitch_speed_pid, motor_pitch_gimbal.fdb.rotor_speed);
pitch_voltage = pitch_speed_pid.OUT;
pitch_current = pitch_speed_pid.OUT + GIMBAL_PITCH_GRAVITY_COMP;
if (pitch_current > GIMBAL_PITCH_SPEED_OUT_LIMIT)
pitch_current = GIMBAL_PITCH_SPEED_OUT_LIMIT;
else if (pitch_current < -GIMBAL_PITCH_SPEED_OUT_LIMIT)
pitch_current = -GIMBAL_PITCH_SPEED_OUT_LIMIT;
/* 发送:CAN2 0x1FF 槽1 = Yaw 电压;CAN1 0x1FF 槽2 = Pitch 电压,槽3 = 拨弹电流 */
dji_motor_send_current_high(&hcan2, (int16_t)yaw_voltage, 0, 0, 0);
dji_motor_send_current_high(&hcan1, 0, (int16_t)pitch_voltage, rammer_current_shared, 0);
/* 发送:
* CAN2 0x1FE 槽1 = Yaw GM6020 电流(ID 1, 0x205)
* CAN1 0x1FE 槽2 = Pitch GM6020 电流(ID 2, 0x206)
* (拨弹 M2006 现已统一移入 0x200 控制帧槽 3, 由 shoot_send_cmd 统一发送)
*/
dji_motor_send_gm6020_current_low(&hcan2, (int16_t)yaw_current, 0, 0, 0);
dji_motor_send_gm6020_current_low(&hcan1, 0, (int16_t)pitch_current, 0, 0);
}
void shoot_share_rammer_current(int16_t current)
@@ -219,7 +227,8 @@ uint8_t gimbal_motor_offline(void)
void gimbal_output_zero(void)
{
dji_motor_send_current_high(&hcan2, 0, 0, 0, 0);
dji_motor_send_current_high(&hcan1, 0, 0, 0, 0);
dji_motor_send_gm6020_current_low(&hcan2, 0, 0, 0, 0);
dji_motor_send_gm6020_current_low(&hcan1, 0, 0, 0, 0);
dji_motor_send_current_low(&hcan1, 0, 0, 0, 0);
dji_motor_send_current_low(&hcan2, 0, 0, 0, 0);
}
云台/modules/control/gimbal.h
+7 -6
View File
@@ -4,13 +4,14 @@
/* 云台双轴控制(串级 PID)
* Yaw (2026-09 重写):双环都以陀螺仪为反馈——
* 位置环:QEKF 多圈 yaw(°) -> 目标角速度(°/s)
* 速度环:陀螺 z 轴角速度(°/s) -> GM6020 电压
* 速度环:陀螺 z 轴角速度(°/s) -> GM6020 电流(±16384)
* 装配符号由上电自检自动测定(gimbal_detect_yaw_sign),
* 两环共用同一符号,不可能再出现"两传感器符号打架"
* Pitch:位置环用 QEKF 的 Roll(安装方向所致,勿改),速度环转子转速
* 电机: Yaw=GM6020 CAN2 0x205(电压,0x1FF CAN2 槽1)
* Pitch=GM6020 CAN1 0x206(电压,与拨弹共帧 0x1FF CAN1 槽2/3,legacy 方案)
* 摩擦轮 M3508 0x201/0x202 与拨弹 M2006 0x207 的实例也在此初始化 */
* Pitch:位置环用 QEKF 的 Roll(安装方向所致,勿改),速度环转子转速 -> GM6020 电流(±16384)
* 电机: Yaw=GM6020 CAN2 0x205(电流控制, 0x1FE CAN2 槽1)
* Pitch=GM6020 CAN1 0x206(电流控制, 0x1FE CAN1 槽2)
* 拨弹=M2006 CAN2 0x201(电流控制, 0x200 CAN2 槽1)
* 摩擦轮 M3508 0x201/0x202 与拨弹 M2006 的实例也在此初始化 */
#include "dji_motor.h"
@@ -19,7 +20,7 @@ extern dji_motor_t motor_yaw_gimbal; // Yaw GM6020,CAN2 0x205
extern dji_motor_t motor_pitch_gimbal; // Pitch GM6020,CAN1 0x206
extern dji_motor_t motor_friction_l; // 左摩擦轮 M3508,CAN1 0x201
extern dji_motor_t motor_friction_r; // 右摩擦轮 M3508,CAN1 0x202
extern dji_motor_t motor_rammer; // 拨弹 M2006,CAN1 0x207
extern dji_motor_t motor_rammer; // 拨弹 M2006,CAN2 0x201
/* 初始化电机实例与 PID(can_start 之后调用) */
extern void gimbal_motor_init(void);
云台/modules/control/shoot.c
+19 -6
View File
@@ -1,10 +1,10 @@
/* =============================================================================
* shoot.c - 发射机构:摩擦轮 2xM3508(0x200 CAN1 槽1/2)+ 拨弹 M2006
* 逻辑(与 legacy 一致):
* shoot.c - 发射机构:摩擦轮 2xM3508(0x200 CAN1 槽1/2)+ 拨弹 M2006(0x200 CAN2 槽1)
* 逻辑:
* 摩擦轮开关受发射机构上电状态门控(可由 DEBUG_SHOOT_WITHOUT_REFEREE 绕过),
* 开启时联动激光(PC8=TIM3_CH3 PWM,占空比 0/满 对应关/开)
* 拨弹仅在摩擦轮开启后允许;fire=GIMBAL_RAMMER_SPEED(-5400),eject 反向
* 拨弹电流经 shoot_share_rammer_current() 交给云台共帧发送(0x1FF CAN1)
* 摩擦轮通过 0x200(CAN1)发送, 拨弹通过 0x200(CAN2)发送
* ========================================================================== */
#include "shoot.h"
#include "gimbal.h"
@@ -43,7 +43,7 @@ void shoot_init(void)
PID_PositionStructureInit(&rammer_pid, GIMBAL_RAMMER_SPEED);
PID_PositionSetParameter(&rammer_pid, GIMBAL_RAMMER_KP, GIMBAL_RAMMER_KI, GIMBAL_RAMMER_KD);
PID_PositionSetEkRange(&rammer_pid, -20, 20);
PID_PositionSetOUTRange(&rammer_pid, -30000, 30000);
PID_PositionSetOUTRange(&rammer_pid, -10000, 10000); // M2006 电流限幅(±10000 对应 ±10A)
laser_init();
}
@@ -84,18 +84,31 @@ void shoot_control(uint8_t friction_on, uint8_t fire, uint8_t eject, uint8_t sho
PID_PositionCalc(&friction_r_pid, motor_friction_r.fdb.rotor_speed);
PID_PositionCalc(&rammer_pid, motor_rammer.fdb.rotor_speed);
/* 拨弹电流交给云台控制帧统一发送(0x1FF CAN1 共帧) */
/* 拨弹电流由 shoot_send_cmd 统一在 0x200 帧(槽 3)发送, 保留共享函数以兼容 */
shoot_share_rammer_current((int16_t)rammer_pid.OUT);
}
void shoot_send_cmd(void)
{
dji_motor_send_current_low(&hcan1, (int16_t)friction_l_pid.OUT, (int16_t)friction_r_pid.OUT, 0, 0);
/* CAN1 0x200: 槽1=左摩擦轮, 槽2=右摩擦轮 */
dji_motor_send_current_low(&hcan1,
(int16_t)friction_l_pid.OUT,
(int16_t)friction_r_pid.OUT,
0,
0);
/* CAN2 0x200: 槽1=拨弹盘 M2006 (ID 1) */
dji_motor_send_current_low(&hcan2,
(int16_t)rammer_pid.OUT,
0,
0,
0);
}
void shoot_output_zero(void)
{
dji_motor_send_current_low(&hcan1, 0, 0, 0, 0);
dji_motor_send_current_low(&hcan2, 0, 0, 0, 0);
LASER_OFF();
shoot_friction_flag = 0;
}
云台/modules/motor/dji_motor.c
+33 -3
View File
@@ -7,8 +7,8 @@
* 记录 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_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
+8 -1
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)
* 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 发送(板间通讯等复用) */
底盘/Application/robot_param.h
+26 -13
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 ============================ */
#define GIMBAL_RAMMER_CAN_ID 0x201 // 拨弹盘 M2006 反馈 ID(CAN2 0x200 控制帧槽 1, 对应电机拨码 ID 1)
/* 云台板:CAN1 = Pitch GM6020(0x206) + 摩擦轮 M3508(0x201/0x202)
* CAN2 = Yaw GM6020(0x205)+底盘回传(0x019)
* CAN2 = Yaw GM6020(0x205) + 拨弹 M2006(0x201) + 底盘回传(0x019)
* 底盘板:CAN1 = 四轮 M3508(0x201~0x204)
* CAN2 = 云台数据(0x149/0x189/0x1A9) */
底盘/modules/motor/dji_motor.c
+33 -3
View File
@@ -7,8 +7,8 @@
* 记录 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_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
+8 -1
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)
* 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 发送(板间通讯等复用) */
电机配置.md
+151
View File
@@ -0,0 +1,151 @@
# Simba-OmniInfantry 电机配置与 CAN 通信地址总表
本文档汇总整车（云台主控板 + 底盘主控板）所有电机的硬件拨码、通信总线分配、CAN 报文帧头（StdId）、数据槽位（Byte 位）、控制模式与发包上下限。
---
## 1. 整车物理通信拓扑
整车采用双主控板（云台板 + 底盘板）分布式架构，物理走线分为三组总线回路：
```mermaid
graph TD
subgraph 云台主控板
G_CAN1[云台 CAN1 独立总线]
G_CAN2[云台 CAN2 总线]
end
subgraph 底盘主控板
C_CAN1[底盘 CAN1 独立总线]
C_CAN2[底盘 CAN2 总线]
end
G_CAN1 --- M_FL[左摩擦轮 M3508]
G_CAN1 --- M_FR[右摩擦轮 M3508]
G_CAN1 --- M_PIT[Pitch 轴 GM6020]
C_CAN1 --- W_RF[右前轮 RF M3508]
C_CAN1 --- W_LF[左前轮 LF M3508]
C_CAN1 --- W_LR[左后轮 LR M3508]
C_CAN1 --- W_RR[右后轮 RR M3508]
G_CAN2 ===|跨板双绞线互联| C_CAN2
G_CAN2 --- M_YAW[Yaw 轴 GM6020]
G_CAN2 --- M_RAM[拨弹盘 M2006]
G_CAN2 -.- C_TOC[CToC 板间通讯数据帧]
```
- **云台 CAN1（独立总线）**：摩擦轮（2×M3508）+ 俯仰 Pitch（GM6020 电流环）；
- **底盘 CAN1（独立总线）**：四轮麦克纳姆轮驱动（4×M3508）；
- **云台 CAN2 ⟷ 底盘 CAN2（跨板直连互联总线）**：偏航 Yaw（GM6020 电流环）+ 拨弹（M2006）+ 底盘/云台板间通讯（CToC）。
---
## 2. 各物理总线电机配置明细表
### 2.1 【云台 CAN1】发射与俯仰机构
| 机构名称 | 电机型号 | 电调/电机拨码 | 反馈 ID (Rx StdId) | 控制帧头 (Tx StdId) | 控制数据槽位 (Bytes) | 控制模式 | 发包上下限 (数值) | 物理对应量程 | 代码发送函数 |
| :--- | :---: | :---: | :---: | :---: | :---: | :---: | :---: | :---: | :--- |
| **左摩擦轮** | M3508 | **ID 1** | `0x201` | **`0x200`** | 槽 1 (`DATA[0..1]`) | 速度环→电流 | `[-15000, 15000]` | $\pm 18.3\,\text{A}$ | `shoot_send_cmd()` |
| **右摩擦轮** | M3508 | **ID 2** | `0x202` | **`0x200`** | 槽 2 (`DATA[2..3]`) | 速度环→电流 | `[-15000, 15000]` | $\pm 18.3\,\text{A}$ | `shoot_send_cmd()` |
| **Pitch 轴** | GM6020 | **ID 2** | `0x206` | **`0x1FE`** | 槽 2 (`DATA[2..3]`) | **电流环** | **`[-16384, 16384]`** | $\pm 3.0\,\text{A}$ | `gimbal_control()` |
> **云台 CAN1 下发报文打包格式**：
> - **`0x200` 帧**：`[左摩擦轮电流 (int16), 右摩擦轮电流 (int16), 0, 0]`（2ms 周期）
> - **`0x1FE` 帧**：`[0, Pitch轴电流 (int16), 0, 0]`（2ms 周期）
---
### 2.2 【底盘 CAN1】四轮底盘行走机构
| 机构名称 | 电机型号 | 电调拨码 | 反馈 ID (Rx StdId) | 控制帧头 (Tx StdId) | 控制数据槽位 (Bytes) | 控制模式 | 发包上下限 (数值) | 物理对应量程 | 代码发送函数 |
| :--- | :---: | :---: | :---: | :---: | :---: | :---: | :---: | :---: | :--- |
| **右前轮 (RF)** | M3508 | **ID 1** | `0x201` | **`0x200`** | 槽 1 (`DATA[0..1]`) | 速度环→电流 | `[-15000, 15000]` | $\pm 18.3\,\text{A}$ | `chassis_control()` |
| **左前轮 (LF)** | M3508 | **ID 2** | `0x202` | **`0x200`** | 槽 2 (`DATA[2..3]`) | 速度环→电流 | `[-15000, 15000]` | $\pm 18.3\,\text{A}$ | `chassis_control()` |
| **左后轮 (LR)** | M3508 | **ID 3** | `0x203` | **`0x200`** | 槽 3 (`DATA[4..5]`) | 速度环→电流 | `[-15000, 15000]` | $\pm 18.3\,\text{A}$ | `chassis_control()` |
| **右后轮 (RR)** | M3508 | **ID 4** | `0x204` | **`0x200`** | 槽 4 (`DATA[6..7]`) | 速度环→电流 | `[-15000, 15000]` | $\pm 18.3\,\text{A}$ | `chassis_control()` |
> **底盘 CAN1 下发报文打包格式**：
> - **`0x200` 帧**：`[RF轮电流 (int16), LF轮电流 (int16), LR轮电流 (int16), RR轮电流 (int16)]`（2ms 周期）
---
### 2.3 【云台 CAN2 ⟷ 底盘 CAN2】互联总线（偏航、拨弹与板间通信）
此总线为**跨板物理直连**，挂载了 2 台电机以及板间双向通讯帧。所有报文 ID 全局唯一、互不干扰：
| 通信对象 / 机构 | 设备类型 | 硬件拨码 ID | 报文帧 ID (StdId) | 方向 / 槽位 | 控制量限幅 (数值) | 报文功能与用途说明 |
| :--- | :---: | :---: | :---: | :---: | :---: | :--- |
| **拨弹盘控制** | M2006 | **ID 1** | **`0x200`** | 云台下发 / 槽 1 (`DATA[0..1]`) | `[-10000, 10000]` | **拨弹电流**（$\pm 10.0\,\text{A}$） |
| **Yaw 轴控制** | GM6020 | **ID 1** | **`0x1FE`** | 云台下发 / 槽 1 (`DATA[0..1]`) | **`[-16384, 16384]`** | **Yaw 电流**（$\pm 3.0\,\text{A}$） |
| **拨弹盘反馈** | M2006 | ID 1 | **`0x201`** | 电机回传 → 云台板 | — | M2006 角度、转速、转矩反馈 |
| **Yaw 轴反馈** | GM6020 | ID 1 | **`0x205`** | 电机回传 → 双板接收 | — | GM6020 角度反馈（云台闭环 + 底盘跟随） |
| **底盘状态回传** | CToC 数据 | — | **`0x019`** | 底盘板 → 云台板 | — | 发射机构上电状态、底盘离线/故障位 |
| **遥控摇杆数据** | CToC 数据 | — | **`0x149`** | 云台板 → 底盘板 | — | DR16 遥控器四路摇杆通道量 |
| **控制状态信号** | CToC 数据 | — | **`0x189`** | 云台板 → 底盘板 | — | 遥控状态、小陀螺使能、云台故障位 |
| **世界系姿态角** | CToC 数据 | — | **`0x1A9`** | 云台板 → 底盘板 | — | 云台世界系 yaw（float）供底盘双 INS 跟随 |
> **互联总线零冲突机制**：
> - **控制下发**：M2006 接收 `0x200` 槽 1；GM6020 接收 `0x1FE` 槽 1，电调/电机内部硬件过滤器各自接收，互不干扰；
> - **反馈回传**：M2006 回传 `0x201`；GM6020 回传 `0x205`；CToC 回传 `0x019`，仲裁 ID 全网唯一。
---
## 3. 电机硬件拨码与接线操作防错清单
| 电机机构名称 | 电机型号 | 物理接线接口 | 硬件拨码开关设定 | 对应反馈 ID | 调车注意事项 |
| :--- | :---: | :---: | :---: | :---: | :--- |
| **左摩擦轮** | M3508 (C620) | 云台板 **CAN1** | **ID 1** | `0x201` | 保持不变 |
| **右摩擦轮** | M3508 (C620) | 云台板 **CAN1** | **ID 2** | `0x202` | 保持不变 |
| **Pitch 轴** | GM6020 | 云台板 **CAN1** | **ID 2** | `0x206` | 保持不变 |
| **Yaw 轴** | GM6020 | 云台板 **CAN2** | **ID 1** | `0x205` | 保持不变 |
| **拨弹盘** | M2006 (C610) | 云台板 **CAN2** | **ID 1** | `0x201` | ⚠️ **需修改：拨码从 7 改为 1，插头换插到云台 CAN2** |
| **底盘右前 (RF)** | M3508 (C620) | 底盘板 **CAN1** | **ID 1** | `0x201` | 保持不变 |
| **底盘左前 (LF)** | M3508 (C620) | 底盘板 **CAN1** | **ID 2** | `0x202` | 保持不变 |
| **底盘左后 (LR)** | M3508 (C620) | 底盘板 **CAN1** | **ID 3** | `0x203` | 保持不变 |
| **底盘右后 (RR)** | M3508 (C620) | 底盘板 **CAN1** | **ID 4** | `0x204` | 保持不变 |
---
## 4. 大疆 DJI 电机 CAN 协议控制帧标准速查
### 4.1 控制帧标识符（StdId）与数据槽位定义
| 控制标识符 (StdId) | 目标电机类型与编号 | 槽 1 (`DATA[0..1]`) | 槽 2 (`DATA[2..3]`) | 槽 3 (`DATA[4..5]`) | 槽 4 (`DATA[6..7]`) | 数据取值范围 |
| :---: | :--- | :---: | :---: | :---: | :---: | :---: |
| **`0x200`** | M3508 / M2006 (ID 1 ~ 4) | 电机 ID 1 | 电机 ID 2 | 电机 ID 3 | 电机 ID 4 | `[-16384, 16384]` (电流) |
| **`0x1FF`** | M3508 / M2006 (ID 5 ~ 8) | 电机 ID 5 | 电机 ID 6 | 电机 ID 7 | 电机 ID 8 | `[-16384, 16384]` (电流) |
| **`0x1FE`** | **GM6020 (ID 1 ~ 4) 电流控制** | **GM6020 ID 1** | **GM6020 ID 2** | **GM6020 ID 3** | **GM6020 ID 4** | **`[-16384, 16384]` (电流)** |
| **`0x2FE`** | **GM6020 (ID 5 ~ 7) 电流控制** | **GM6020 ID 5** | **GM6020 ID 6** | **GM6020 ID 7** | 保留 | **`[-16384, 16384]` (电流)** |
| *`0x1FF`* | *GM6020 (ID 1 ~ 4) 旧电压控制* | *GM6020 ID 1* | *GM6020 ID 2* | *GM6020 ID 3* | *GM6020 ID 4* | *`[-25000, 25000]` (电压)* |
| *`0x2FF`* | *GM6020 (ID 5 ~ 7) 旧电压控制* | *GM6020 ID 5* | *GM6020 ID 6* | *GM6020 ID 7* | *保留* | *`[-25000, 25000]` (电压)* |
### 4.2 电机反馈报文格式（Rx 反馈，全电机统一 DLC = 8）
```text
DATA[0..1] : 机械角度 raw_angle (uint16_t, 0 ~ 8191 对应 0 ~ 360°)
DATA[2..3] : 转子转速 rotor_speed (int16_t, 单位 RPM)
DATA[4..5] : 实际转矩 raw_torque (int16_t, 反馈转矩电流值)
DATA[6] : 电机温度 temperature (uint8_t, 单位 ℃)
DATA[7] : 保留字节 (NULL)
```
---
## 5. 代码实现与参数映射对照
### 5.1 参数配置文件：`robot_param.h`
- 拨弹盘 CAN 反馈 ID：
```c
#define GIMBAL_RAMMER_CAN_ID 0x201 // M2006 反馈 ID (CAN2 0x200 帧槽 1)
```
- Pitch 轴电流环限幅：
```c
#define GIMBAL_PITCH_SPEED_OUT_LIMIT 16384.0f // GM6020 电流满量程 (±3A)
```
- Yaw 轴电流环限幅：
```c
#define GIMBAL_YAW_SPEED_OUT_LIMIT 16384.0f // GM6020 电流满量程 (±3A)
```