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
+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) */