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
云台/modules/control/gimbal.c
+145 -37
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,29 +195,120 @@ 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;
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)
{
/* 堵转计时:相对基准编码值移动超过 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);
}
void shoot_share_rammer_current(int16_t current)
#if 0
/* ===== 2026-10-15 台架调参临时接口(已停用,保留备查) ===== */
/* Pitch 位置环实际设定值(°) = 真实目标角 + 经限位门控后的台架偏置 */
float gimbal_pitch_setpoint_deg(void)
{
rammer_current_shared = current;
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)
{
uint32_t now = HAL_GetTick();
@@ -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
+32 -10
View File
@@ -6,9 +6,8 @@
* (无 legacy 的 +-1 圈限制,底盘小陀螺时 Yaw 可连续旋转),
* 记录 last_online_tick 供掉线检测
* 控制路径(任务上下文):
* dji_motor_send_current_low/high() 0x200 / 0x1FF 电流帧(+/-16384 = 20A/10A)
* dji_motor_send_voltage_high() 0x2FF 电压帧(GM6020 ID5~7)
* 注意 0x1FF 帧为 M3508/M2006 高位电流与 GM6020 低位电压共用,按槽位区分
* dji_motor_send_current_low/high() 0x200 / 0x1FF 电流帧(+/-16384 = 20A/10A)
* dji_motor_send_gm6020_current_low/high() 0x1FE / 0x2FE GM6020 电流帧(+/-16384 = 3A)
* ========================================================================== */
#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]);