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
云台/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;
}