ELCT/Simba-OmniInfantry
9
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

修复上电云台正方向问题，以及初步调好了底盘跟随PID，以及小陀螺模式下的运动偏移问题。

Browse Source
This commit is contained in:
lyf
2026-09-12 19:05:26 +08:00
parent cf736569fb
commit 79a545b379
14 changed files with 266 additions and 71 deletions
Download Patch File Download Diff File Expand all files Collapse all files
底盘/Application/Task/Src/chassis_task.c
+10 -1
View File
@@ -17,6 +17,7 @@
#include "robot_typedef.h"
#include "chassis.h"
#include "attitude.h"
#include "bmi088.h"
#include "referee.h"
#include "c_comm.h"
#include "robot_param.h"
@@ -120,6 +121,14 @@ void chassis_task(void const * argument)
float vx = cmd_robot.vx * boost;
float vy = cmd_robot.vy * boost;
/* 坐标系旋转投影:将以云台枪口为准的平移速度转换到底盘坐标系，并叠加 kw 前馈 */
float chassis_vx = vx;
float chassis_vy = vy;
/* 跟随模式下无持续自旋，前馈量为0；小陀螺模式下代入实际自旋角速度 w */
float w_feedforward = (cmd_robot.chassis_mode == CHASSIS_MODE_FOLLOW) ? 0.0f : bmi088_gyro[2];
chassis_vector_to_body(vx, vy, w_feedforward, &chassis_vx, &chassis_vy);
/* 简易功率限制:缓冲能量线性映射为缩放系数 */
float scale = 1.0f;
if (referee.buffer_energy < POWER_LIMIT_BUFFER_LOW)
@@ -130,7 +139,7 @@ void chassis_task(void const * argument)
else if (scale > 1.0f)
scale = 1.0f;
chassis_control(vx, vy, w, scale);
chassis_control(chassis_vx, chassis_vy, w, scale);
}
else
{
底盘/Application/robot_param.h
+9 -5
View File
@@ -12,6 +12,8 @@
#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 4440 // Yaw 轴编码器零位(机械正前方,0~8191)
#define CHASSIS_SPIN_FEEDFORWARD_K 0.0725f // 小陀螺解算时延前馈补偿系数(s)
#define GIMBAL_FRICTION_WHEEL_SPEED 5500.0f // 摩擦轮转速(RPM)
#define GIMBAL_RAMMER_SPEED -5400.0f// 拨弹盘转速(RPM,负向发射)
@@ -31,16 +33,17 @@
* (GIMBAL_YAW_DETECT 见下),无需人工猜;
* - 单位:角度环 误差° -> 目标角速度°/s;速度环 误差°/s -> 电压。
* 调参顺序:先速度环(点动不振荡),再位置环(刚性)——见移植 README §8.1 */
#define GIMBAL_YAW_ANGLE_KP 15.0f // ° -> °/s:10° 误差给 150°/s
#define GIMBAL_YAW_ANGLE_KP 20.0f // ° -> °/s:10° 误差给 150°/s
#define GIMBAL_YAW_ANGLE_KI 0.0f
#define GIMBAL_YAW_ANGLE_KD 0.0f // 速度环本身即微分,不再叠加
#define GIMBAL_YAW_ANGLE_EK_DEADBAND 0.05f // 误差死区(°),QEKF 噪声小可用窄死区
#define GIMBAL_YAW_ANGLE_OUT_LIMIT 360.0f // 目标角速度上限(°/s)
#define GIMBAL_YAW_SPEED_KP 60.0f // °/s -> 电压:100°/s 误差给 6000(约 24% 满电压)
#define GIMBAL_YAW_SPEED_KP 85.0f // °/s -> 电压:100°/s 误差给 6000(约 24% 满电压)
#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)
/* Yaw 装配符号自动检测:上电自检时给 Yaw 电机一个正向小电压脉冲,
* 看陀螺 z 轴往哪边转——脉冲引起同向转动则符号 +1,反向则 -1。
@@ -91,10 +94,10 @@
* err = wrap180(云台yaw - 底盘yaw)
* 不再依赖底盘侧 GM6020(0x205) 反馈,皮带虚位不在环内,无基准锁定时序坑。
* 两板 INS 各自上电零点任意,但差值只随相对转动变化,天然正确 */
#define CHASSIS_FOLLOW_KP 0.4f // w 量纲/°:10° 偏差给 w=4
#define CHASSIS_FOLLOW_KP 0.4678f // w 量纲/°:10° 偏差给 w=4
#define CHASSIS_FOLLOW_KI 0.0f // 调试期 0,同旧教训(积分残留)
#define CHASSIS_FOLLOW_KD 0.0f
#define CHASSIS_FOLLOW_OUT_LIMIT 4.0f // 输出限幅(介于慢/快陀螺之间)
#define CHASSIS_FOLLOW_KD 24.452f
#define CHASSIS_FOLLOW_OUT_LIMIT 4.5f // 输出限幅(介于慢/快陀螺之间)
#define CHASSIS_FOLLOW_DEADBAND 1.0f // 误差死区(°)
/* 跟随符号全自动综合:云台自检(0x1A9 flags)下发编码器 tick 增向,
@@ -129,6 +132,7 @@
* 干扰)。红灯常亮与其它灯语完全不变,仍能看出是哪台设备掉线;比赛请改 0。
* 旋律音符表见 modules/BUZZER/buzzer.c 的 buzzer_melody[] */
#define DEBUG_BUZZER_MELODY 1
#define BUZZER_MELODY_SPEED_DIV 1 // 旋律降速分频(默认1:每步100ms精准匹配原版带吐音断音节拍) // 旋律降速分频(默认2:放慢一倍匹配原曲抒情节拍;设为3/4可进一步放慢)
/* ============================ 掉线检测 ============================ */
#define DEVICE_OFFLINE_TIME_MS 100 // CAN 设备/裁判系统掉线判定时间
底盘/MDK-ARM/Simba-ELCT-FrameWork.uvoptx
+1 -1
View File
@@ -103,7 +103,7 @@
<bEvRecOn>1</bEvRecOn>
<bSchkAxf>0</bSchkAxf>
<bTchkAxf>0</bTchkAxf>
<nTsel>3</nTsel>
<nTsel>2</nTsel>
<sDll></sDll>
<sDllPa></sDllPa>
<sDlgDll></sDlgDll>
底盘/modules/control/chassis.c
+51 -13
View File
@@ -14,6 +14,7 @@
* 功率: chassis_control() 的 power_scale 直接缩放四轮输出电流
* ========================================================================== */
#include "chassis.h"
#include <math.h>
#include "pid.h"
#include "filter.h"
#include "attitude.h"
@@ -153,32 +154,30 @@ float chassis_follow_sign(void)
return dbg_follow_sign * (float)ctoc_encoder_ccw;
}
/* 跟随一步:反馈为 Yaw GM6020 编码器相对角(零漂移),输出 w */
/* 跟随一步:反馈为 Yaw GM6020 编码器对齐设定机械零位(4050),输出 w */
float chassis_follow_w(void)
{
/* 尚未收到 0x205 反馈时不锁定基准(避免首帧假偏差),输出 0 */
/* 尚未收到 0x205 反馈时输出 0 */
if (motor_yaw_follow.fdb.last_online_tick == 0)
{
follow_latched = 0;
return 0;
}
if (follow_reset_request || !follow_latched)
if (follow_reset_request)
{
follow_reset_request = 0;
follow_target_tick = (float)motor_yaw_follow.fdb.total_tick;
follow_latched = 1;
PID_PositionClean(&follow_pid);
}
/* 计算当前机械编码值距离设定零位(4050)的最短差值(-4096 ~ +4096) */
int16_t tick_diff = (int16_t)motor_yaw_follow.fdb.raw_angle - (int16_t)YAW_GM6020_ENCODER_ZERO;
if (tick_diff > 4096)
tick_diff -= 8192;
else if (tick_diff < -4096)
tick_diff += 8192;
/* 相对角误差(°):tick 增向按云台自检结果折算为 CCW 正 */
float err_deg = ((float)motor_yaw_follow.fdb.total_tick - follow_target_tick) * 360.0f / 8192.0f;
float err_deg = (float)tick_diff * (360.0f / 8192.0f);
if (ctoc_encoder_ccw < 0)
err_deg = -err_deg;
if (err_deg > 180.0f)
err_deg -= 360.0f;
else if (err_deg < -180.0f)
err_deg += 360.0f;
if (err_deg > -CHASSIS_FOLLOW_DEADBAND && err_deg < CHASSIS_FOLLOW_DEADBAND)
err_deg = 0;
@@ -195,3 +194,42 @@ void chassis_output_zero(void)
{
dji_motor_send_current_low(&hcan1, 0, 0, 0, 0);
}
#ifndef PI
#define PI 3.14159265358979323846f
#endif
void chassis_vector_to_body(float vx_g, float vy_g, float w, float * vx_c, float * vy_c)
{
/* 电机掉线兜底:若无云台编码器反馈,直接直通输出 */
if (motor_yaw_follow.fdb.last_online_tick == 0)
{
*vx_c = vx_g;
*vy_c = vy_g;
return;
}
/* 1. 计算当前机械编码值距离设定零位的最短差值(-4096 ~ +4096) */
int16_t tick_diff = (int16_t)motor_yaw_follow.fdb.raw_angle - (int16_t)YAW_GM6020_ENCODER_ZERO;
if (tick_diff > 4096)
tick_diff -= 8192;
else if (tick_diff < -4096)
tick_diff += 8192;
/* 2. 相对机械角误差(rad):tick 增向按云台自检结果折算为 CCW 正 */
float alpha_rad = (float)tick_diff * (2.0f * PI / 8192.0f);
int8_t enc_sign = (ctoc_encoder_ccw != 0) ? ctoc_encoder_ccw : 1;
if (enc_sign < 0)
alpha_rad = -alpha_rad;
/* 3. 叠加角速度前馈补偿(-kw 消除通信及响应时延滞后) */
float alpha_comp = alpha_rad - (CHASSIS_SPIN_FEEDFORWARD_K * w);
/* 4. 二维旋转矩阵投影(修正正负号: 适配 vx<0前进, vy>0向左 的坐标系定义) */
float cos_a = cosf(alpha_comp);
float sin_a = sinf(alpha_comp);
*vx_c = vx_g * cos_a + vy_g * sin_a;
*vy_c = -vx_g * sin_a + vy_g * cos_a;
}
底盘/modules/control/chassis.h
+5 -1
View File
@@ -46,4 +46,8 @@ extern volatile float dbg_follow_err_deg;
extern volatile float dbg_follow_out;
extern volatile float dbg_follow_sign;
#endif
/* 将以云台为基准的平移速度(vx_g, vy_g)旋转投影到底盘自身坐标系(vx_c, vy_c)
* 无论底盘是否处于小陀螺自旋状态，使 vx_g 始终代表沿云台枪口方向前进 */
extern void chassis_vector_to_body(float vx_g, float vy_g, float w, float * vx_c, float * vy_c);
#endif
底盘/modules/dev/buzzer.c
+63 -10
View File
@@ -6,6 +6,7 @@
* ========================================================================== */
#include "buzzer.h"
#include "tim.h"
#include "robot_param.h"
extern TIM_HandleTypeDef htim4;
@@ -32,7 +33,8 @@ void buzzer_on(buzzer_tone_e tone)
{
__HAL_TIM_SET_PRESCALER(&htim4, buzzer_tone_freq[tone]);
HAL_TIM_GenerateEvent(&htim4, TIM_EVENTSOURCE_UPDATE); // 立即装载 PSC
__HAL_TIM_SET_COMPARE(&htim4, TIM_CHANNEL_3, 5); // 占空比 50%
/* 占空比 30%:抑制高次尖锐谐波,使吉他拨弦音色圆润纯净 */
__HAL_TIM_SET_COMPARE(&htim4, TIM_CHANNEL_3, 3);
}
else
__HAL_TIM_SET_COMPARE(&htim4, TIM_CHANNEL_3, 0);
@@ -44,21 +46,72 @@ void buzzer_off(void)
}
/* ==========================================================================
* 调试用旋律(见 buzzer.h):每步 100ms,空拍 BUZZER_P 为静音。
* 无源蜂鸣器只有单音,这里用"上行分解和弦 + 回落"拼成一句短旋律;选音限定
* 在中音 3 ~ 高音 2,避免刺耳。循环播放不会漏掉"有设备掉线"这一事实,
* 因为红灯常亮由 monitor_task 独立控制,与该旋律无关。
* 改旋律:直接增删这里的音符即可,长度会自动生效。
* 周杰伦《晴天》- 标志性吉他前奏伴奏 Solo (纯享循环版)
* 调式: 1=C,处于 C 板蜂鸣器最佳频响区间(500~1000Hz)
* 结构: 完整还原原曲前奏 4 小节经典木吉他单音分解
* [小节1: Em] 3 2 1 1 2 1 5_ 1 2
* [小节2: C ] 3 2 1 1 2 1 6_ 1 2
* [小节3: G ] 3 2 1 1 2 1 5_ 1 2
* [小节4: D ] 3 2 1 1 2 1 2 3 5 3 2 1 2 -
* 吐音设计: 每个音符保留 BUZZER_P 间隔,模拟吉他真实的清脆拨弦与泛音
* ========================================================================== */
static const buzzer_tone_e buzzer_melody[] = {
BUZZER_M3, BUZZER_M5, BUZZER_M6, BUZZER_H1, BUZZER_P,
BUZZER_M6, BUZZER_H1, BUZZER_H2, BUZZER_P,
BUZZER_H1, BUZZER_M6, BUZZER_M5, BUZZER_M3, BUZZER_P,
/* ---- 小节 1 (Em): 3 2 1 1 2 1 | 5_ 1 2 ---- */
BUZZER_M3, BUZZER_P, // 3 (Mi)
BUZZER_M2, BUZZER_P, // 2 (Re)
BUZZER_M1, BUZZER_P, // 1 (Do)
BUZZER_M1, // 1 (Do)
BUZZER_M2, // 2 (Re)
BUZZER_M1, BUZZER_P, // 1 (Do)
BUZZER_L5, BUZZER_P, // 低音 5 (Sol)
BUZZER_M1, // 1 (Do)
BUZZER_M2, // 2 (Re)
/* ---- 小节 2 (C): 3 2 1 1 2 1 | 6_ 1 2 ---- */
BUZZER_M3, BUZZER_P, // 3 (Mi)
BUZZER_M2, BUZZER_P, // 2 (Re)
BUZZER_M1, BUZZER_P, // 1 (Do)
BUZZER_M1, // 1 (Do)
BUZZER_M2, // 2 (Re)
BUZZER_M1, BUZZER_P, // 1 (Do)
BUZZER_L6, BUZZER_P, // 低音 6 (La)
BUZZER_M1, // 1 (Do)
BUZZER_M2, // 2 (Re)
/* ---- 小节 3 (G): 3 2 1 1 2 1 | 5_ 1 2 ---- */
BUZZER_M3, BUZZER_P, // 3 (Mi)
BUZZER_M2, BUZZER_P, // 2 (Re)
BUZZER_M1, BUZZER_P, // 1 (Do)
BUZZER_M1, // 1 (Do)
BUZZER_M2, // 2 (Re)
BUZZER_M1, BUZZER_P, // 1 (Do)
BUZZER_L5, BUZZER_P, // 低音 5 (Sol)
BUZZER_M1, // 1 (Do)
BUZZER_M2, // 2 (Re)
/* ---- 小节 4 (D): 3 2 1 1 2 1 | 2 3 5 3 2 1 2 - ---- */
BUZZER_M3, BUZZER_P, // 3 (Mi)
BUZZER_M2, BUZZER_P, // 2 (Re)
BUZZER_M1, BUZZER_P, // 1 (Do)
BUZZER_M1, // 1 (Do)
BUZZER_M2, // 2 (Re)
BUZZER_M1, BUZZER_P, // 1 (Do)
BUZZER_M2, BUZZER_P, // 2 (Re)
BUZZER_M3, // 3 (Mi)
BUZZER_M5, // 5 (Sol)
BUZZER_M3, // 3 (Mi)
BUZZER_M2, // 2 (Re)
BUZZER_M1, BUZZER_P, // 1 (Do)
BUZZER_M2, BUZZER_M2, BUZZER_M2, BUZZER_M2, BUZZER_M2, BUZZER_P, // 2 (Re 长音结音)
BUZZER_P, BUZZER_P, BUZZER_P, BUZZER_P, // 尾部休止 400ms 后循环
};
void buzzer_play_melody(uint16_t step)
{
const uint16_t steps =
(uint16_t)(sizeof(buzzer_melody) / sizeof(buzzer_melody[0]));
buzzer_on(buzzer_melody[step % steps]);
#ifndef BUZZER_MELODY_SPEED_DIV
#define BUZZER_MELODY_SPEED_DIV 1
#endif
buzzer_on(buzzer_melody[(step / BUZZER_MELODY_SPEED_DIV) % steps]);
}