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-16 00:54:52 +08:00
parent 67eea77789
commit 370d4519b5
20 changed files with 477 additions and 163 deletions
Download Patch File Download Diff File Expand all files Collapse all files
.gitignore
+14
View File
@@ -28,6 +28,20 @@
*.uvguix
DebugConfig/
# ---- J-Link 调试器自动生成(与具体探针/机器绑定, 非源码) ----
# JLinkSettings.ini: 点"下载/调试"时 Keil 写下的连接参数(器件/接口/速率)
# JLinkLog.txt: J-Link 驱动日志
# 两者都会随换机器、换探针、换烧录方式而变, 不应入库; 缺失时 Keil 会自动重建
JLinkSettings.ini
JLinkLog.txt
JLinkLog.txt.*
*.jlink
# ---- Keil UV4 编译/烧录日志(uv4 -b/-f -o 生成) ----
build_log.txt
*_build.txt
*_flash.txt
# ---- 编辑器 / 系统 ----
*.bak
*.ddk
README.md
+1 -1
View File
@@ -175,7 +175,7 @@ FreeRTOS 节拍 1 kHz。任务来自 `Core/Src/freertos.c`:
| 板 | CAN1 | CAN2 |
| --- | --- | --- |
| 云台板 | Pitch GM6020 `0x206`、摩擦轮 M3508 `0x201/0x202` | Yaw GM6020 `0x205`、底盘回传 `0x019` |
| 云台板 | Pitch GM6020 `0x206`(0x1FE)、摩擦轮 M3508 `0x201/0x202`(0x200) | Yaw GM6020 `0x205`(0x1FE)、拨弹 M2006 `0x201`(0x200)、底盘回传 `0x019` |
| 底盘板 | 四轮 M3508 `0x201~0x204` | 云台数据 `0x149/0x189/0x1A9` |
### 6.2 串口分配
云台/Application/Task/Src/debug_task.c
+50 -6
View File
@@ -36,24 +36,66 @@
#include "ins.h"
#include "bmi088.h"
#include "gimbal.h"
#include "dr16.h"
#include <stdio.h>
#include <math.h>
#include <string.h>
/* ===== 2026-10-15 台架调参临时遥测(已停用,保留备查) =====
* 启用时把 #if 0 改 #if 1:本任务改为输出 5 列 CSV
* setpoint_deg, actual_deg, out_current, ecd_deg, gyro_x_dps
* (逗号分隔+换行,可被 Lissio FireWater 自动识别为 5 通道),采样率提到 200Hz,
* 用于定量观测阶跃响应与判断"轴是否真的在转动"。 */
#if 0
#include "robot_param.h"
#define DEBUG_PITCH_TUNE 1
#if DEBUG_PITCH_TUNE
#define DEBUG_OUT_PERIOD_MS 5 /* 200Hz:2ms 控制环的阶跃响应需 ≥100Hz 采样才量得准 */
#endif
#endif
/* 1-输出健康位(cvg/rdy/imu/an/gn); 0-只输出三个欧拉角 */
#define DEBUG_ATT_HEALTH 1
#define DEBUG_ATT_HEALTH 0
/* 1-再附加原始陀螺/零偏/采样计数(排障用); 0-只留上面的通用健康位 */
#define DEBUG_ATT_DIAG 1
#define DEBUG_ATT_DIAG 0
/* 1-仅输出 Pitch 调参行(便于用 VOFA+/串口示波器绘图); 0-同时输出姿态行与 Pitch 调参行 */
#define DEBUG_PITCH_ONLY 0
/* 输出周期(ms)。姿态观测建议 100ms(10Hz) */
#define DEBUG_OUT_PERIOD_MS 100
void debug_task(void const * argument)
{
char line[192];
char line[384];
char tail[112];
char pitch_line[128];
for (;;)
{
#if 0
/* ===== 2026-10-15 台架 Pitch 调参遥测(已停用,保留备查) =====
* 5 列 CSV → Lissio FireWater 直接成 5 通道:
* setpoint(位置环设定值含台架偏置), actual(-att_deg_roll),
* out(最终电流), ecd_deg(编码器机械角), gyro_x_dps(陀螺 x 轴角速度) */
const float tune_ecd_deg = ((float)motor_pitch_gimbal.fdb.raw_angle -
(float)PITCH_GM6020_ENCODER_ZERO) * (360.0f / 8192.0f);
const float tune_gyro_dps = bmi088_gyro[0] * 57.29578f;
int len = snprintf(line, sizeof(line), "%.3f,%.3f,%.1f,%.3f,%.2f\n",
gimbal_pitch_setpoint_deg(),
-att_deg_roll,
pitch_current,
tune_ecd_deg,
tune_gyro_dps);
#endif
#if DEBUG_PITCH_ONLY
/* 仅输出 Pitch 调参数据(编码值、roll、PID最后输出值、遥控pitch杆量) */
int len = snprintf(line, sizeof(line),
"p_ecd=%u roll=%.2f p_out=%.1f rc_pitch=%u\r\n",
(unsigned)motor_pitch_gimbal.fdb.raw_angle,
att_deg_roll,
pitch_current,
(unsigned)dr16.rc_l_UD);
#else
const float an = sqrtf(bmi088_accel[0] * bmi088_accel[0] +
bmi088_accel[1] * bmi088_accel[1] +
bmi088_accel[2] * bmi088_accel[2]);
@@ -79,9 +121,11 @@ void debug_task(void const * argument)
(unsigned)INS.ConvergeFlag, (unsigned)att_ready,
(unsigned)bmi088_online, an, gn, tail);
#else
int len = snprintf(line, sizeof(line), "ecd=%u yaw=%.2f pitch=%.2f roll=%.2f\r\n",
(unsigned)motor_yaw_gimbal.fdb.raw_angle,
att_deg_yaw, att_deg_pitch, att_deg_roll);
int len = snprintf(line, sizeof(line), "%u,%.1f,%u\r\n",
(unsigned)motor_pitch_gimbal.fdb.raw_angle,
pitch_current,(unsigned)motor_yaw_gimbal.fdb.raw_angle);
#endif
#endif
if (len > 0)
云台/Application/Task/Src/gimbal_task.c
+30 -2
View File
@@ -5,10 +5,10 @@
* YAW_RUN 正常串级控制(符号用参数回退值,直至手势自检完成)
* YAW_DETECT 装配符号自检——**仅由操作手手势显式触发,绝不自动执行**:
* 遥控在线时"左摇杆推到最左 + 右摇杆推到最右"保持 0.5s。
* 检测期间云台会轻转几度(约 20% 电压脉冲 60ms),操作手应
* 检测期间云台会轻转几度(约 20% 电流脉冲 60ms),操作手应
* 确认云台周围无人后再做手势。失败重试 3 次后回退参数符号。
* 任意门控失效(遥控离线/电机掉线/姿态不可用/底盘故障)立即清零输出
* 拨弹电流由 shoot_task 写入共享变量,本任务与 Pitch 共帧发送(0x1FF CAN1)
* Yaw/Pitch GM6020 采用 0x1FE 电流帧控制;拨弹 M2006 由 shoot_task 独立发 0x200
* ========================================================================== */
#include "app_tasks.h"
#include "cmsis_os.h"
@@ -64,6 +64,34 @@ void gimbal_task(void const * argument)
if (gesture)
gesture_cnt = 0;
#if 0
/* ===== 2026-10-15 台架 Pitch 阶跃发生器(已停用,保留备查) =====
* **有界**测试,不是持续抖动:只跑 GIMBAL_BENCH_PITCH_HALF_COUNT 个半周期,
* 跑完自动归零并锁存,之后云台恢复静止(重新上电/改开关才会再跑一次)。
* 配合 gimbal.c 的台架偏置使用;2ms 任务,半周期 tick 数 = HALF_MS / 2 */
{
static uint32_t bench_phase = 0;
static uint8_t bench_done = 0;
const uint32_t half_ticks = (uint32_t)GIMBAL_BENCH_PITCH_HALF_MS / 2u;
if (!bench_done && half_ticks > 0u)
{
const uint32_t total_ticks = half_ticks * (uint32_t)GIMBAL_BENCH_PITCH_HALF_COUNT;
if (bench_phase >= total_ticks)
{
bench_done = 1;
gimbal_bench_pitch_offset = 0.0f; /* 跑完立即停,不再抖动 */
}
else
{
gimbal_bench_pitch_offset = ((bench_phase / half_ticks) % 2u)
? GIMBAL_BENCH_PITCH_STEP_DEG
: 0.0f;
bench_phase++;
}
}
}
#endif
switch (state)
{
case YAW_WAIT:
云台/Application/Task/Src/monitor_task.c
+2 -2
View File
@@ -85,8 +85,8 @@ static int8_t linkcheck_first_offline(void)
{
uint32_t now = HAL_GetTick();
#if defined(ROBOT_TYPE_GIMBAL)
/* 设备序号:0 摩擦轮L(0x201) 1 摩擦轮R(0x202) 2 Pitch(0x206)
* 3 Yaw(0x205) 4 拨弹(0x207,安装时) 5 底盘回传(0x019) */
/* 设备序号:0 摩擦轮L(CAN1 0x201) 1 摩擦轮R(CAN1 0x202) 2 Pitch(CAN1 0x206)
* 3 Yaw(CAN2 0x205) 4 拨弹(CAN2 0x201,安装时) 5 底盘回传(0x019) */
if (LINK_OFFLINE(motor_friction_l.fdb.last_online_tick))
return 0;
if (LINK_OFFLINE(motor_friction_r.fdb.last_online_tick))
云台/Application/Task/Src/robot_cmd_task.c
+7 -7
View File
@@ -97,8 +97,8 @@ void robot_cmd_task(void const * argument)
if (!keyboard_online)
{
/* 遥控器:左摇杆,输出与摇杆偏量成比例(legacy 为过死区即全速,
* 体感过灵敏),限位门控与方向保持 legacy */
/* 遥控器:左摇杆向上(>1050)为抬头(pitch_delta减小,pos向2260减小),门控须 pos > UPPER_LIMIT(2260);
* 左摇杆向下(<1000)为低头(pitch_delta增大,pos向3950增大),门控须 pos < LOWER_LIMIT(3950) */
if (dr16.rc_l_UD > 1050 && pos > PITCH_GM6020_POSITION_UPPER_LIMIT)
pitch_delta -= GIMBAL_PITCH_LEVER_RATE / 8192.0f * 360.0f *
((float)dr16.rc_l_UD - 1024.0f) / 660.0f;
@@ -113,13 +113,13 @@ void robot_cmd_task(void const * argument)
}
else
{
/* 键鼠:同改为比例输出 */
if (keyboard_data.mouse_du < -8 && pos > PITCH_GM6020_POSITION_UPPER_LIMIT)
/* 键鼠:鼠标前推(>8)为抬头(pitch_delta减小),门控须 pos > UPPER_LIMIT;鼠标后拉(<-8)为低头(pitch_delta增大),门控须 pos < LOWER_LIMIT */
if (keyboard_data.mouse_du > 8 && pos > PITCH_GM6020_POSITION_UPPER_LIMIT)
pitch_delta -= GIMBAL_PITCH_LEVER_RATE / 8192.0f * 360.0f *
(-(float)keyboard_data.mouse_du * PC_MOUSE_DU_SENSITIVITY) / 660.0f;
else if (keyboard_data.mouse_du > 8 && pos < PITCH_GM6020_POSITION_LOWER_LIMIT)
pitch_delta += GIMBAL_PITCH_LEVER_RATE / 8192.0f * 360.0f *
((float)keyboard_data.mouse_du * PC_MOUSE_DU_SENSITIVITY) / 660.0f;
else if (keyboard_data.mouse_du < -8 && pos < PITCH_GM6020_POSITION_LOWER_LIMIT)
pitch_delta += GIMBAL_PITCH_LEVER_RATE / 8192.0f * 360.0f *
(-(float)keyboard_data.mouse_du * PC_MOUSE_DU_SENSITIVITY) / 660.0f;
if (keyboard_data.mouse_rl != 0)
yaw_delta -= GIMBAL_LEVER_SPEED_MAP_RATE * GIMBAL_YAW_PITCH_SPEED_RATE *
云台/Application/Task/Src/shoot_task.c
+2 -2
View File
@@ -1,7 +1,7 @@
/* =============================================================================
* shoot_task.c - 发射机构任务(2ms)
* shoot_control() 计算摩擦轮/拨弹电流 -> shoot_send_cmd() 发 0x200;
* 拨弹电流由云台任务与 Pitch 共帧发送;发射机构上电状态来自底盘 0x019
* shoot_control() 计算摩擦轮/拨弹电流 -> shoot_send_cmd() 发 0x200(CAN1 摩擦轮 / CAN2 拨弹);
* 发射机构上电状态来自底盘 0x019
* ========================================================================== */
#include "app_tasks.h"
#include "cmsis_os.h"
云台/Application/robot_param.h
+74 -17
View File
@@ -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
云台/README.md
+5 -5
View File
@@ -8,8 +8,8 @@
## 这块板负责什么
- **云台控制**:Yaw(GM6020 `0x205`,CAN2)/ Pitch(GM6020 `0x206`,CAN1)双环
- **发射机构**:摩擦轮 M3508 `0x201`/`0x202` + 拨弹 M2006 `0x207`(装与不装由 `GIMBAL_RAMMER_INSTALLED` 决定)
- **云台控制**:Yaw(GM6020 `0x205`,CAN2 `0x1FE`电流控制)/ Pitch(GM6020 `0x206`,CAN1 `0x1FE`电流控制)双环
- **发射机构**:摩擦轮 M3508 `0x201`/`0x202`(CAN1 `0x200`) + 拨弹 M2006 `0x201`(CAN2 `0x200`)(装与不装由 `GIMBAL_RAMMER_INSTALLED` 决定)
- **图传链路 / 裁判系统**:USART6(3pin 口)
- **DR16 遥控器**:USART3
- **板间通讯主控**:云台把世界系 yaw(`0x1A9`)与摇杆/控制状态(`0x149`/`0x189`)发给底盘
@@ -38,7 +38,7 @@ USE_HAL_DRIVER, STM32F407xx, ARM_MATH_CM4, ROBOT_TYPE_GIMBAL
1. `led_init` 点亮蓝灯 → 各外设初始化
2. IMU 初始化 + **z 轴零偏静止标定(约 0.5~1.5 s,这段时间别碰车)**
3. **Yaw 装配符号自检**:给 Yaw 电机一个正向小电压脉冲,看陀螺 z 轴往哪边转,自动定符号;**云台会轻转几度,不要扶住云台**。失败(被按住 / 电机离线)时回退到 `GIMBAL_YAW_SIGN_FALLBACK`
3. **Yaw 装配符号自检**:给 Yaw 电机一个正向小电流脉冲,看陀螺 z 轴往哪边转,自动定符号;**云台会轻转几度,不要扶住云台**。失败(被按住 / 电机离线)时回退到 `GIMBAL_YAW_SIGN_FALLBACK`
4. 自检结果经 `0x1A9` 的 flags 下发给底盘,与底盘自测的车身转向相乘完成跟随符号综合
## 蜂鸣器序号(本板)
@@ -46,8 +46,8 @@ USE_HAL_DRIVER, STM32F407xx, ARM_MATH_CM4, ROBOT_TYPE_GIMBAL
CAN 掉线报警按设备序号响 `n` 下:
```
0 = 摩擦轮L(0x201) 1 = 摩擦轮R(0x202) 2 = Pitch(0x206)
3 = Yaw(0x205) 4 = 拨弹(0x207) 5 = 底盘回传(0x019)
0 = 摩擦轮L(CAN1 0x201) 1 = 摩擦轮R(CAN1 0x202) 2 = Pitch(CAN1 0x206)
3 = Yaw(CAN2 0x205) 4 = 拨弹(CAN2 0x201) 5 = 底盘回传(0x019)
```
**响三下 = Pitch 电机掉线。** 台架调试嫌吵时把 `robot_param.h` 的 `DEBUG_BUZZER_MELODY` 置 1(只换声音不换灯语)。
云台/modules/control/gimbal.c
+146 -38
View File
@@ -2,11 +2,11 @@
* 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 -> 电流(限幅)
* 稳定性条件:SIGN*G>0,G 为"电流->陀螺 z 角速度"的装配增益,
* 由 gimbal_detect_yaw_sign() 上电实测,彻底消除双传感器符号打架
* Pitch:位置环用 QEKF Roll(安装方向),速度环转子转速(legacy 案,无符号史问题)
* 发送:CAN2 0x1FF 槽1=Yaw 电压;CAN1 0x1FF 槽2=Pitch 电压,槽3=拨弹电流
* 发送:CAN2 0x1FE 槽1=Yaw 电流;CAN1 0x1FE 槽2=Pitch 电流
* ========================================================================== */
#include "gimbal.h"
#include "shoot.h"
@@ -18,11 +18,16 @@
#include "filter.h"
#include "robot_typedef.h"
#include "dr16.h"
#include <math.h>
#ifndef RAD_TO_ANGLE
#define RAD_TO_ANGLE 57.295779513082320876798154814105f
#endif
#ifndef ANGLE_TO_RAD
#define ANGLE_TO_RAD 0.01745329251994329576923690768489f
#endif
dji_motor_t motor_yaw_gimbal;
dji_motor_t motor_pitch_gimbal;
dji_motor_t motor_friction_l;
@@ -34,25 +39,32 @@ 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;
float pitch_pid = 0.0f;
float pitch_g = 0.0f;
float pitch_current = 0.0f;
/* ===== 2026-10-15 台架调参临时量(已停用,保留备查) ===== */
#if 0
/* 台架调参用 Pitch 目标角附加偏置(°)。由 gimbal_task 的方波发生器写入,
* 只叠加到位置环设定值上,不累加进 pitch_target_deg,默认 0 无副作用 */
float gimbal_bench_pitch_offset = 0.0f;
/* 实际施加的偏置(经限位安全门控后),仅用于遥测显示真实设定值 */
static float pitch_applied_offset = 0.0f;
#endif
/* 装配符号:0=未检测,±1=已测定;未检测前 gimbal_control 不应被调用 */
static float yaw_sign_f = 0.0f;
static uint8_t yaw_sign_valid = 0;
/* 拨弹电流由 shoot 模块写入,云台与拨弹共帧(0x1FF CAN1)发送 */
static int16_t rammer_current_shared = 0;
void shoot_share_rammer_current(int16_t current);
void gimbal_motor_init(void)
{
dji_motor_init(&motor_yaw_gimbal, &hcan2, 0x205, DJI_MOTOR_6020);
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, 0x201, DJI_MOTOR_2006);
/* Yaw 位置环:° -> °/s */
PID_PositionStructureInit(&yaw_angle_pid, 0);
@@ -60,7 +72,7 @@ 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);
@@ -73,8 +85,8 @@ void gimbal_motor_init(void)
PID_PositionStructureInit(&pitch_speed_pid, 300);
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, 0, 0);
PID_PositionSetOUTRange(&pitch_speed_pid, -16384, 16384);
}
uint8_t gimbal_detect_yaw_sign(void)
@@ -83,24 +95,24 @@ 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, 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;
if (rate_dps > GIMBAL_YAW_DETECT_RATE_MIN)
{
yaw_sign_f = 1.0f; // 正电压->陀螺正转,装配增益 G>0
yaw_sign_f = 1.0f; // 正电流->陀螺正转,装配增益 G>0
yaw_sign_valid = 1;
}
else if (rate_dps < -GIMBAL_YAW_DETECT_RATE_MIN)
@@ -121,7 +133,7 @@ float gimbal_yaw_sign(void)
int8_t gimbal_encoder_ccw(void)
{
/* 自检:电压正脉冲 -> 陀螺 z 正转(云台 CCW),而电压正方向即编码器
/* 自检:电流正脉冲 -> 陀螺 z 正转(云台 CCW),而电流正方向即编码器
* tick 增大方向,故 tick 增方向 = sign(gimbal 旋转) */
if (!yaw_sign_valid)
return 0;
@@ -151,7 +163,11 @@ void gimbal_control(float yaw_delta, float pitch_delta, uint8_t reset_flag)
/* 将编码器 tick 差折算为机械角度偏差(°)，补偿给世界系目标角 */
const float ecd_err_deg = (float)ecd_diff * (360.0f / 8192.0f);
yaw_target_deg = yaw_meas - ecd_err_deg;
pitch_target_deg = 0;
/* Pitch: 计算当前编码器距离机械零位的差值，折算为角度偏差补偿给目标角(上电自动归位到零位) */
int16_t pitch_ecd_diff = (int16_t)motor_pitch_gimbal.fdb.raw_angle - (int16_t)PITCH_GM6020_ENCODER_ZERO;
const float pitch_ecd_err_deg = (float)pitch_ecd_diff * (360.0f / 8192.0f);
pitch_target_deg = -att_deg_roll - pitch_ecd_err_deg;
if (reset_flag)
{
@@ -169,7 +185,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,28 +195,119 @@ 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 */
#if 0
/* ===== 2026-10-15 台架调参临时改动(已停用,保留备查) =====
* 启用时:先算编码器角与上下软限位,做台架偏置的限位安全门控(轴距限位
* 不足 GIMBAL_BENCH_PITCH_SAFE_MARGIN_DEG 时不施加阶跃),并把下面一行
* 改为 pitch_angle_pid.Need_Value = pitch_target_deg + pitch_applied_offset; */
const float pitch_ecd_deg = ((float)motor_pitch_gimbal.fdb.raw_angle -
(float)PITCH_GM6020_ENCODER_ZERO) * (360.0f / 8192.0f);
const float pitch_lim_up_deg = ((float)PITCH_GM6020_POSITION_UPPER_LIMIT -
(float)PITCH_GM6020_ENCODER_ZERO) * (360.0f / 8192.0f);
const float pitch_lim_dn_deg = ((float)PITCH_GM6020_POSITION_LOWER_LIMIT -
(float)PITCH_GM6020_ENCODER_ZERO) * (360.0f / 8192.0f);
pitch_applied_offset = gimbal_bench_pitch_offset;
if (pitch_ecd_deg < pitch_lim_up_deg + GIMBAL_BENCH_PITCH_SAFE_MARGIN_DEG ||
pitch_ecd_deg > pitch_lim_dn_deg - GIMBAL_BENCH_PITCH_SAFE_MARGIN_DEG)
pitch_applied_offset = 0.0f;
#endif
pitch_angle_pid.Need_Value = pitch_target_deg;
PID_PositionCalc(&pitch_angle_pid, att_deg_roll);
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_pid = pitch_speed_pid.OUT;
/* 发送: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);
}
/* Pitch 重力拟合前馈补偿:
* a 为当前物理编码值减去 3200*360/8192 的数值(折算为弧度送入三角函数)
* I = 3105.88 * cos(a) + 3565.50 * sin(a) + 64.31
* 最终电流输出 pitch_out = pitch_pid + pitch_g (速度环不代入 pitch_g) */
const float a_deg = ((float)motor_pitch_gimbal.fdb.raw_angle - GIMBAL_PITCH_GRAVITY_ZERO_ECD) * (360.0f / 8192.0f);
const float a_rad = a_deg * ANGLE_TO_RAD;
pitch_g = GIMBAL_PITCH_GRAVITY_K_COS * cosf(a_rad) +
GIMBAL_PITCH_GRAVITY_K_SIN * sinf(a_rad) +
GIMBAL_PITCH_GRAVITY_K_CONST;
void shoot_share_rammer_current(int16_t current)
pitch_current = pitch_pid + pitch_g;
if (pitch_current > 16384.0f)
pitch_current = 16384.0f;
else if (pitch_current < -16384.0f)
pitch_current = -16384.0f;
#if 0
/* ===== 2026-10-15 台架调参临时改动:Pitch 软限位/堵转保护(已停用,保留备查) =====
* 背景:原设计的上下限位只作用于遥控输入路径(robot_cmd_task),控制环自身的
* 目标角/前馈没有任何限位约束。台架烧录会失力矩十几秒~几分钟,轴会掉到限位,
* 若不加保护,原逻辑会持续输出大电流顶着限位(GM6020 直驱堵转 = 持续过载发热)。
* 做法:越限位时只允许"离开限位"方向的电流(正电流 = 抬头/编码器减小,故下限位
* 只留正向、上限位只留负向),反方向清零;限位外连续 GIMBAL_PITCH_LIMIT_STALL_MS
* 编码器不动则判堵转,电流降到涓流上限。轴回到软限位内后闭环自动恢复正常。 */
if (pitch_ecd_deg >= pitch_lim_dn_deg || pitch_ecd_deg <= pitch_lim_up_deg)
{
rammer_current_shared = current;
/* 堵转计时:相对基准编码值移动超过 10 个刻度(0.44°)才算"在动"。
* 阈值取 10 而不是 2:陀螺/结构微振会让编码器抖 2~3 个刻度,阈值太小会把
* 静止误判成在动,保护永远不生效(实测踩过);慢速抬升 5°/s 在 300ms 内
* 也能走 34 个刻度,不会被误判。 */
static uint16_t lim_stall = 0;
static uint16_t lim_ref_ecd = 0;
const int16_t d_ecd = (int16_t)((int16_t)motor_pitch_gimbal.fdb.raw_angle -
(int16_t)lim_ref_ecd);
if (d_ecd > 10 || d_ecd < -10)
{
lim_ref_ecd = motor_pitch_gimbal.fdb.raw_angle;
lim_stall = 0;
}
else if (lim_stall < 60000u)
lim_stall++;
const float cap = (lim_stall * 2u >= (uint16_t)GIMBAL_PITCH_LIMIT_STALL_MS)
? GIMBAL_PITCH_LIMIT_CURRENT
: GIMBAL_PITCH_LIMIT_CURRENT_RECOVER;
if (pitch_ecd_deg >= pitch_lim_dn_deg) /* 轴过低:只许抬升 */
{
if (pitch_current < 0.0f)
pitch_current = 0.0f;
else if (pitch_current > cap)
pitch_current = cap;
}
else /* 轴过高:只许下压 */
{
if (pitch_current > 0.0f)
pitch_current = 0.0f;
else if (pitch_current < -cap)
pitch_current = -cap;
}
PID_PositionClean(&pitch_speed_pid);
}
#endif
/* ===== 停用块结束 ===== */
/* 发送:CAN2 0x1FE 槽1 = Yaw 电流;CAN1 0x1FE 槽2 = Pitch 电流 */
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);
}
#if 0
/* ===== 2026-10-15 台架调参临时接口(已停用,保留备查) ===== */
/* Pitch 位置环实际设定值(°) = 真实目标角 + 经限位门控后的台架偏置 */
float gimbal_pitch_setpoint_deg(void)
{
return pitch_target_deg + pitch_applied_offset;
}
/* Pitch 位置环输出 = 速度环目标(°/s),用于判断内环是否被限幅 */
float gimbal_pitch_speed_ref(void)
{
return pitch_angle_pid.OUT;
}
#endif
uint8_t gimbal_motor_offline(void)
{
@@ -219,7 +326,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
+20 -8
View File
@@ -4,13 +4,13 @@
/* 云台双轴控制(串级 PID)
* Yaw (2026-09 重写):双环都以陀螺仪为反馈——
* 位置环:QEKF 多圈 yaw(°) -> 目标角速度(°/s)
* 速度环:陀螺 z 轴角速度(°/s) -> GM6020 电压
* 速度环:陀螺 z 轴角速度(°/s) -> GM6020 电流
* 装配符号由上电自检自动测定(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 的实例也在此初始化 */
* 电机: Yaw=GM6020 CAN2 0x205(电流,0x1FE CAN2 槽1)
* Pitch=GM6020 CAN1 0x206(电流,0x1FE CAN1 槽2)
* 摩擦轮 M3508 0x201/0x202(CAN1) 与拨弹 M2006 0x201(CAN2) 的实例也在此初始化 */
#include "dji_motor.h"
@@ -19,12 +19,15 @@ 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
extern float pitch_pid; // Pitch PID 输出分量(仅速度环反馈计算)
extern float pitch_g; // Pitch 重力拟合前馈分量
extern float pitch_current; // Pitch GM6020 最终电流控制输出(pitch_pid + pitch_g, ±16384)
/* 初始化电机实例与 PID(can_start 之后调用) */
extern void gimbal_motor_init(void);
/* Yaw 装配符号自检:给 Yaw 电机正向小电压脉冲,按陀螺 z 轴响应方向
/* Yaw 装配符号自检:给 Yaw 电机正向小电流脉冲,按陀螺 z 轴响应方向
* 测定符号。阻塞约 200ms(HAL_Delay),检测期间云台会轻转几度。
* 返回 1-成功(gimbal_yaw_sign() 可用) 0-失败(被按住/电机离线/无响应) */
extern uint8_t gimbal_detect_yaw_sign(void);
@@ -36,13 +39,22 @@ extern float gimbal_yaw_sign(void);
* 0=尚未自检。随 0x1A9 flags bit1 下发,供底盘跟随符号自动综合 */
extern int8_t gimbal_encoder_ccw(void);
/* 云台轴控制:根据 cmd 增量更新目标角并计算电压输出(2ms 调用)
* 内部发送 CAN2 0x1FF(Yaw)与 CAN1 0x1FF(Pitch+拨弹) */
/* 云台轴控制:根据 cmd 增量更新目标角并计算电流输出(2ms 调用)
* 内部发送 CAN2 0x1FE(Yaw)与 CAN1 0x1FE(Pitch) */
extern void gimbal_control(float yaw_delta, float pitch_delta, uint8_t reset_flag);
/* 云台任一电机掉线返回 1 */
extern uint8_t gimbal_motor_offline(void);
/* ===== 2026-10-15 台架调参临时接口(已停用,保留备查) =====
* 启用时把 #if 0 改 #if 1,并同步启用 gimbal.c / gimbal_task.c / robot_param.h
* / debug_task.c 里的同名停用块 */
#if 0
extern float gimbal_bench_pitch_offset; // 位置环设定值附加偏置(°),台架方波发生器写入
extern float gimbal_pitch_setpoint_deg(void); // 位置环实际设定值(°),供调参遥测读取
extern float gimbal_pitch_speed_ref(void); // 位置环输出=速度环目标(°/s),判断内环是否限幅
#endif
/* 云台输出清零(停车) */
extern void gimbal_output_zero(void);
云台/modules/control/shoot.c
+8 -7
View File
@@ -1,10 +1,10 @@
/* =============================================================================
* shoot.c - 发射机构:摩擦轮 2xM3508(0x200 CAN1 槽1/2)+ 拨弹 M2006
* shoot.c - 发射机构:摩擦轮 2xM3508(0x200 CAN1 槽1/2)+ 拨弹 M2006(0x200 CAN2 槽1)
* 逻辑(与 legacy 一致):
* 摩擦轮开关受发射机构上电状态门控(可由 DEBUG_SHOOT_WITHOUT_REFEREE 绕过),
* 开启时联动激光(PC8=TIM3_CH3 PWM,占空比 0/满 对应关/开)
* 拨弹仅在摩擦轮开启后允许;fire=GIMBAL_RAMMER_SPEED(-5400),eject 反向
* 拨弹电流经 shoot_share_rammer_current() 交给云台共帧发送(0x1FF CAN1)
* 拨弹电流经 shoot_send_cmd() 经 CAN2 0x200 帧直接发送(ID 1)
* ========================================================================== */
#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);
laser_init();
}
@@ -67,7 +67,7 @@ void shoot_control(uint8_t friction_on, uint8_t fire, uint8_t eject, uint8_t sho
shoot_friction_flag = 0;
}
/* 拨弹:仅在摩擦轮开启时允许 */
/* 拨弹:仅在摩擦轮开启后允许 */
if (shoot_friction_flag)
{
if (fire)
@@ -83,19 +83,20 @@ void shoot_control(uint8_t friction_on, uint8_t fire, uint8_t eject, uint8_t sho
PID_PositionCalc(&friction_l_pid, motor_friction_l.fdb.rotor_speed);
PID_PositionCalc(&friction_r_pid, motor_friction_r.fdb.rotor_speed);
PID_PositionCalc(&rammer_pid, motor_rammer.fdb.rotor_speed);
/* 拨弹电流交给云台控制帧统一发送(0x1FF CAN1 共帧) */
shoot_share_rammer_current((int16_t)rammer_pid.OUT);
}
void shoot_send_cmd(void)
{
/* 摩擦轮 2×M3508:CAN1 0x200 槽1/2 */
dji_motor_send_current_low(&hcan1, (int16_t)friction_l_pid.OUT, (int16_t)friction_r_pid.OUT, 0, 0);
/* 拨弹盘 M2006(ID 1):CAN2 0x200 槽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/control/shoot.h
+2 -4
View File
@@ -3,7 +3,7 @@
#include <stdint.h>
/* 发射机构:摩擦轮 2×M3508(0x200 CAN1 槽1/2)+ 拨弹 M2006(电流由云台共帧发送)
/* 发射机构:摩擦轮 2×M3508(0x200 CAN1 槽1/2)+ 拨弹 M2006(0x200 CAN2 槽1)
* 摩擦轮开启时联动激光(PC8,经 TIM3 CH3 PWM) */
/* 初始化摩擦轮/拨弹 PID(can_start 之后调用) */
@@ -14,7 +14,7 @@ extern void shoot_init(void);
* shooter_power_on:发射机构上电状态(来自底盘 0x019) */
extern void shoot_control(uint8_t friction_on, uint8_t fire, uint8_t eject, uint8_t shooter_power_on);
/* 发送摩擦轮电流帧(0x200 CAN1) */
/* 发送摩擦轮(0x200 CAN1)与拨弹(0x200 CAN2)电流帧 */
extern void shoot_send_cmd(void);
/* 摩擦轮输出清零 */
@@ -23,6 +23,4 @@ extern void shoot_output_zero(void);
/* 摩擦轮当前开关状态(供激光联动与调试) */
extern uint8_t shoot_friction_flag;
/* 拨弹电流写入共享变量(由 shoot 模块调用,云台共帧发送) */
extern void shoot_share_rammer_current(int16_t current);
#endif
云台/modules/motor/dji_motor.c
+31 -9
View File
@@ -7,8 +7,7 @@
* 记录 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)
* ========================================================================== */
#include "dji_motor.h"
@@ -20,7 +19,8 @@
static dji_motor_t * dji_motor_list[DJI_MOTOR_MAX];
static uint8_t dji_motor_count = 0;
static void dji_motor_rx_callback(uint32_t std_id, const uint8_t rx_data[8], uint32_t dlc);
static void dji_motor_can1_rx_callback(uint32_t std_id, const uint8_t rx_data[8], uint32_t dlc);
static void dji_motor_can2_rx_callback(uint32_t std_id, const uint8_t rx_data[8], uint32_t dlc);
/* 解析一帧反馈,多圈累计(无 ±1 圈限制,底盘小陀螺时 Yaw 电机可连续旋转) */
static void dji_motor_parse(dji_motor_t * motor, const uint8_t data[8])
@@ -71,16 +71,32 @@ void dji_motor_init(dji_motor_t * motor, hcan_t * hcan, uint16_t rx_id, dji_moto
if (dji_motor_count < DJI_MOTOR_MAX)
{
dji_motor_list[dji_motor_count++] = motor;
can_rx_register(hcan, rx_id, dji_motor_rx_callback);
if (hcan == &hcan1)
can_rx_register(hcan, rx_id, dji_motor_can1_rx_callback);
else
can_rx_register(hcan, rx_id, dji_motor_can2_rx_callback);
}
}
static void dji_motor_rx_callback(uint32_t std_id, const uint8_t rx_data[8], uint32_t dlc)
static void dji_motor_can1_rx_callback(uint32_t std_id, const uint8_t rx_data[8], uint32_t dlc)
{
(void)dlc;
for (uint8_t i = 0; i < dji_motor_count; i++)
{
if (dji_motor_list[i]->rx_id == std_id)
if (dji_motor_list[i]->hcan == &hcan1 && dji_motor_list[i]->rx_id == std_id)
{
dji_motor_parse(dji_motor_list[i], rx_data);
return;
}
}
}
static void dji_motor_can2_rx_callback(uint32_t std_id, const uint8_t rx_data[8], uint32_t dlc)
{
(void)dlc;
for (uint8_t i = 0; i < dji_motor_count; i++)
{
if (dji_motor_list[i]->hcan == &hcan2 && dji_motor_list[i]->rx_id == std_id)
{
dji_motor_parse(dji_motor_list[i], rx_data);
return;
@@ -119,8 +135,14 @@ 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_voltage_high(hcan_t * hcan, int16_t v5, int16_t v6, int16_t v7)
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] = {v5, v6, v7, 0};
dji_motor_send_int16x4(hcan, 0x2FF, v);
const int16_t v[4] = {i1, i2, i3, i4};
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, int16_t i8)
{
const int16_t v[4] = {i5, i6, i7, i8};
dji_motor_send_int16x4(hcan, 0x2FE, v);
}
云台/modules/motor/dji_motor.h
+7 -6
View File
@@ -5,13 +5,13 @@
/* DJI 电机(M3508/M2006/GM6020)统一驱动
* - 反馈:can_start() 后通过 can_rx_register 按 rx_id 注册,接收中断内解析(多圈累计)
* - 控制:组装 0x200/0x1FF/0x2FF 控制帧,经 CAN_SendTxMessage 发送 */
* - 控制:组装 0x200/0x1FF/0x1FE/0x2FE 控制帧,经 CAN_SendTxMessage 发送 */
typedef enum
{
DJI_MOTOR_3508, // 减速比 3591:187(≈19:1), 转矩常数 0.3N·m/A
DJI_MOTOR_2006, // 减速比 36:1, 转矩常数 0.18N·m/A
DJI_MOTOR_6020, // 直驱, 电压控制
DJI_MOTOR_6020, // 直驱, 电流控制(ID1~4 为 0x1FE 帧, ±16384 对应 ±3A)
} dji_motor_type_e;
typedef struct
@@ -20,7 +20,7 @@ typedef struct
int64_t total_tick; // 累计角度位置原始数据(8192*圈数+raw,无 ±1 圈限制)
float rotor_angle_deg; // 累计转子机械角(°)
int16_t rotor_speed; // 转子转速(RPM)
int16_t raw_torque; // 转矩电流/转矩电压原始数据
int16_t raw_torque; // 转矩电流原始数据
float torque_current; // 转矩电流(A)
uint8_t temperature; // 电机温度(℃)
uint32_t last_online_tick; // 最近一次收到反馈的 tick(HAL_GetTick)
@@ -39,11 +39,12 @@ 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)
* GM6020 电流控制:ID1~4 为 0x1FE 电流帧(±16384 对应 ±3A),ID5~8 为 0x2FE 电流帧(±16384) */
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_voltage_high(hcan_t * hcan, int16_t v5, int16_t v6, int16_t v7);
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, int16_t i8);
/* 通用 int16x4 发送(板间通讯等复用) */
extern void dji_motor_send_int16x4(hcan_t * hcan, uint32_t std_id, const int16_t val[4]);
底盘/Application/Task/Src/monitor_task.c
+2 -2
View File
@@ -85,8 +85,8 @@ static int8_t linkcheck_first_offline(void)
{
uint32_t now = HAL_GetTick();
#if defined(ROBOT_TYPE_GIMBAL)
/* 设备序号:0 摩擦轮L(0x201) 1 摩擦轮R(0x202) 2 Pitch(0x206)
* 3 Yaw(0x205) 4 拨弹(0x207,安装时) 5 底盘回传(0x019) */
/* 设备序号:0 摩擦轮L(CAN1 0x201) 1 摩擦轮R(CAN1 0x202) 2 Pitch(CAN1 0x206)
* 3 Yaw(CAN2 0x205) 4 拨弹(CAN2 0x201,安装时) 5 底盘回传(0x019) */
if (LINK_OFFLINE(motor_friction_l.fdb.last_online_tick))
return 0;
if (LINK_OFFLINE(motor_friction_r.fdb.last_online_tick))
底盘/Application/Task/Src/robot_cmd_task.c
+7 -7
View File
@@ -97,8 +97,8 @@ void robot_cmd_task(void const * argument)
if (!keyboard_online)
{
/* 遥控器:左摇杆,输出与摇杆偏量成比例(legacy 为过死区即全速,
* 体感过灵敏),限位门控与方向保持 legacy */
/* 遥控器:左摇杆向上(>1050)为抬头(pitch_delta减小,pos向2260减小),门控须 pos > UPPER_LIMIT(2260);
* 左摇杆向下(<1000)为低头(pitch_delta增大,pos向3950增大),门控须 pos < LOWER_LIMIT(3950) */
if (dr16.rc_l_UD > 1050 && pos > PITCH_GM6020_POSITION_UPPER_LIMIT)
pitch_delta -= GIMBAL_PITCH_LEVER_RATE / 8192.0f * 360.0f *
((float)dr16.rc_l_UD - 1024.0f) / 660.0f;
@@ -113,13 +113,13 @@ void robot_cmd_task(void const * argument)
}
else
{
/* 键鼠:同改为比例输出 */
if (keyboard_data.mouse_du < -8 && pos > PITCH_GM6020_POSITION_UPPER_LIMIT)
/* 键鼠:鼠标前推(>8)为抬头(pitch_delta减小),门控须 pos > UPPER_LIMIT;鼠标后拉(<-8)为低头(pitch_delta增大),门控须 pos < LOWER_LIMIT */
if (keyboard_data.mouse_du > 8 && pos > PITCH_GM6020_POSITION_UPPER_LIMIT)
pitch_delta -= GIMBAL_PITCH_LEVER_RATE / 8192.0f * 360.0f *
(-(float)keyboard_data.mouse_du * PC_MOUSE_DU_SENSITIVITY) / 660.0f;
else if (keyboard_data.mouse_du > 8 && pos < PITCH_GM6020_POSITION_LOWER_LIMIT)
pitch_delta += GIMBAL_PITCH_LEVER_RATE / 8192.0f * 360.0f *
((float)keyboard_data.mouse_du * PC_MOUSE_DU_SENSITIVITY) / 660.0f;
else if (keyboard_data.mouse_du < -8 && pos < PITCH_GM6020_POSITION_LOWER_LIMIT)
pitch_delta += GIMBAL_PITCH_LEVER_RATE / 8192.0f * 360.0f *
(-(float)keyboard_data.mouse_du * PC_MOUSE_DU_SENSITIVITY) / 660.0f;
if (keyboard_data.mouse_rl != 0)
yaw_delta -= GIMBAL_LEVER_SPEED_MAP_RATE * GIMBAL_YAW_PITCH_SPEED_RATE *
底盘/Application/robot_param.h
+30 -24
View File
@@ -9,11 +9,11 @@
#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 CHASSIS_SPIN_FEEDFORWARD_K 0.0725f // 小陀螺解算时延前馈补偿系数(s)
#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)
#define GIMBAL_RAMMER_SPEED -5400.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,35 @@
#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_KP 15.0f
#define GIMBAL_PITCH_ANGLE_EK_DEADBAND 0.05f // 误差死区(°),收窄死区消除边缘阶跃
#define GIMBAL_PITCH_ANGLE_KP 40.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 100.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 // 常数偏置项
#define GIMBAL_FRICTION_WHEEL_KP 16.0f
#define GIMBAL_FRICTION_WHEEL_KI 0.0f
@@ -97,7 +104,7 @@
#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_OUT_LIMIT 5.5f // 输出限幅(介于慢/快陀螺之间)
#define CHASSIS_FOLLOW_DEADBAND 1.0f // 误差死区(°)
/* 跟随符号全自动综合:云台自检(0x1A9 flags)下发编码器 tick 增向,
@@ -119,8 +126,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,11 +135,10 @@
#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[] */
/* 1=把 CAN 掉线警告音替换成循环播放的旋律《你》(RM动画片尾曲),报警时播放;
* 旋律音符表见 modules/dev/buzzer.c 的 buzzer_melody[] */
#define DEBUG_BUZZER_MELODY 0
#define BUZZER_MELODY_SPEED_DIV 1 // 旋律降速分频(默认1:每步100ms精准匹配原版带吐音断音节拍) // 旋律降速分频(默认2:放慢一倍匹配原曲抒情节拍;设为3/4可进一步放慢)
#define BUZZER_MELODY_SPEED_DIV 1 // 旋律降速分频(默认1:每步100ms精准匹配原版带吐音断音节拍)
/* ============================ 掉线检测 ============================ */
#define DEVICE_OFFLINE_TIME_MS 100 // CAN 设备/裁判系统掉线判定时间
底盘/modules/motor/dji_motor.c
+31 -9
View File
@@ -7,8 +7,7 @@
* 记录 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)
* ========================================================================== */
#include "dji_motor.h"
@@ -20,7 +19,8 @@
static dji_motor_t * dji_motor_list[DJI_MOTOR_MAX];
static uint8_t dji_motor_count = 0;
static void dji_motor_rx_callback(uint32_t std_id, const uint8_t rx_data[8], uint32_t dlc);
static void dji_motor_can1_rx_callback(uint32_t std_id, const uint8_t rx_data[8], uint32_t dlc);
static void dji_motor_can2_rx_callback(uint32_t std_id, const uint8_t rx_data[8], uint32_t dlc);
/* 解析一帧反馈,多圈累计(无 ±1 圈限制,底盘小陀螺时 Yaw 电机可连续旋转) */
static void dji_motor_parse(dji_motor_t * motor, const uint8_t data[8])
@@ -71,16 +71,32 @@ void dji_motor_init(dji_motor_t * motor, hcan_t * hcan, uint16_t rx_id, dji_moto
if (dji_motor_count < DJI_MOTOR_MAX)
{
dji_motor_list[dji_motor_count++] = motor;
can_rx_register(hcan, rx_id, dji_motor_rx_callback);
if (hcan == &hcan1)
can_rx_register(hcan, rx_id, dji_motor_can1_rx_callback);
else
can_rx_register(hcan, rx_id, dji_motor_can2_rx_callback);
}
}
static void dji_motor_rx_callback(uint32_t std_id, const uint8_t rx_data[8], uint32_t dlc)
static void dji_motor_can1_rx_callback(uint32_t std_id, const uint8_t rx_data[8], uint32_t dlc)
{
(void)dlc;
for (uint8_t i = 0; i < dji_motor_count; i++)
{
if (dji_motor_list[i]->rx_id == std_id)
if (dji_motor_list[i]->hcan == &hcan1 && dji_motor_list[i]->rx_id == std_id)
{
dji_motor_parse(dji_motor_list[i], rx_data);
return;
}
}
}
static void dji_motor_can2_rx_callback(uint32_t std_id, const uint8_t rx_data[8], uint32_t dlc)
{
(void)dlc;
for (uint8_t i = 0; i < dji_motor_count; i++)
{
if (dji_motor_list[i]->hcan == &hcan2 && dji_motor_list[i]->rx_id == std_id)
{
dji_motor_parse(dji_motor_list[i], rx_data);
return;
@@ -119,8 +135,14 @@ 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_voltage_high(hcan_t * hcan, int16_t v5, int16_t v6, int16_t v7)
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] = {v5, v6, v7, 0};
dji_motor_send_int16x4(hcan, 0x2FF, v);
const int16_t v[4] = {i1, i2, i3, i4};
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, int16_t i8)
{
const int16_t v[4] = {i5, i6, i7, i8};
dji_motor_send_int16x4(hcan, 0x2FE, v);
}
底盘/modules/motor/dji_motor.h
+7 -6
View File
@@ -5,13 +5,13 @@
/* DJI 电机(M3508/M2006/GM6020)统一驱动
* - 反馈:can_start() 后通过 can_rx_register 按 rx_id 注册,接收中断内解析(多圈累计)
* - 控制:组装 0x200/0x1FF/0x2FF 控制帧,经 CAN_SendTxMessage 发送 */
* - 控制:组装 0x200/0x1FF/0x1FE/0x2FE 控制帧,经 CAN_SendTxMessage 发送 */
typedef enum
{
DJI_MOTOR_3508, // 减速比 3591:187(≈19:1), 转矩常数 0.3N·m/A
DJI_MOTOR_2006, // 减速比 36:1, 转矩常数 0.18N·m/A
DJI_MOTOR_6020, // 直驱, 电压控制
DJI_MOTOR_6020, // 直驱, 电流控制(ID1~4 为 0x1FE 帧, ±16384 对应 ±3A)
} dji_motor_type_e;
typedef struct
@@ -20,7 +20,7 @@ typedef struct
int64_t total_tick; // 累计角度位置原始数据(8192*圈数+raw,无 ±1 圈限制)
float rotor_angle_deg; // 累计转子机械角(°)
int16_t rotor_speed; // 转子转速(RPM)
int16_t raw_torque; // 转矩电流/转矩电压原始数据
int16_t raw_torque; // 转矩电流原始数据
float torque_current; // 转矩电流(A)
uint8_t temperature; // 电机温度(℃)
uint32_t last_online_tick; // 最近一次收到反馈的 tick(HAL_GetTick)
@@ -39,11 +39,12 @@ 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)
* GM6020 电流控制:ID1~4 为 0x1FE 电流帧(±16384 对应 ±3A),ID5~8 为 0x2FE 电流帧(±16384) */
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_voltage_high(hcan_t * hcan, int16_t v5, int16_t v6, int16_t v7);
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, int16_t i8);
/* 通用 int16x4 发送(板间通讯等复用) */
extern void dji_motor_send_int16x4(hcan_t * hcan, uint32_t std_id, const int16_t val[4]);