From 79a545b379cac5eb4dc67dcaf82ca7a746333f29 Mon Sep 17 00:00:00 2001 From: K <169361657@qq.com> Date: Sat, 12 Sep 2026 19:05:26 +0800 Subject: [PATCH] =?UTF-8?q?=E4=BF=AE=E5=A4=8D=E4=B8=8A=E7=94=B5=E4=BA=91?= =?UTF-8?q?=E5=8F=B0=E6=AD=A3=E6=96=B9=E5=90=91=E9=97=AE=E9=A2=98=EF=BC=8C?= =?UTF-8?q?=E4=BB=A5=E5=8F=8A=E5=88=9D=E6=AD=A5=E8=B0=83=E5=A5=BD=E4=BA=86?= =?UTF-8?q?=E5=BA=95=E7=9B=98=E8=B7=9F=E9=9A=8FPID=EF=BC=8C=E4=BB=A5?= =?UTF-8?q?=E5=8F=8A=E5=B0=8F=E9=99=80=E8=9E=BA=E6=A8=A1=E5=BC=8F=E4=B8=8B?= =?UTF-8?q?=E7=9A=84=E8=BF=90=E5=8A=A8=E5=81=8F=E7=A7=BB=E9=97=AE=E9=A2=98?= =?UTF-8?q?=E3=80=82?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .vscode/settings.json | 3 + README.md | 5 ++ 云台/Application/Task/Src/debug_task.c | 7 ++- 云台/Application/Task/Src/gimbal_task.c | 4 +- 云台/Application/robot_param.h | 19 +++--- 云台/MDK-ARM/Simba-ELCT-FrameWork.uvoptx | 6 +- 云台/modules/control/gimbal.c | 50 +++++++++++----- 云台/modules/dev/buzzer.c | 73 ++++++++++++++++++++---- 底盘/Application/Task/Src/chassis_task.c | 11 +++- 底盘/Application/robot_param.h | 14 +++-- 底盘/MDK-ARM/Simba-ELCT-FrameWork.uvoptx | 2 +- 底盘/modules/control/chassis.c | 64 ++++++++++++++++----- 底盘/modules/control/chassis.h | 6 +- 底盘/modules/dev/buzzer.c | 73 ++++++++++++++++++++---- 14 files changed, 266 insertions(+), 71 deletions(-) create mode 100644 .vscode/settings.json diff --git a/.vscode/settings.json b/.vscode/settings.json new file mode 100644 index 0000000..4b7d4d2 --- /dev/null +++ b/.vscode/settings.json @@ -0,0 +1,3 @@ +{ + "python.analysis.stubPath": "C:\\Users\\16936\\.kendryte\\k230_canmv_pylance\\4702c5814722ec2f\\typings" +} \ No newline at end of file diff --git a/README.md b/README.md index c55397e..41689f8 100644 --- a/README.md +++ b/README.md @@ -344,6 +344,9 @@ gx=0.0012 gy=-0.0031 gz=0.0165 zo=0.0009 ck=1 ca=803 cg=1002 se=0 st=1 > 每一条都是实际踩过的坑。更详细的排障过程见 [doc/已知问题与排障.md](./doc/已知问题与排障.md)。 +### 11.0 已知底盘1号电机，云台pitch电机会掉线，记得检修 + + ### 11.1 增删源文件必须同步改 `.uvprojx`(和 `.uvoptx`) 新增/删除 `.c` 后必须同时在 Keil 工程里增删条目,并检查: @@ -431,3 +434,5 @@ BMI088 的 DRDY 是**电平型**信号:某次失败或被挡掉的读取没把 ## 参考 - 踩坑记录与排障手册:[`doc/已知问题与排障.md`](./doc/已知问题与排障.md) + + diff --git a/云台/Application/Task/Src/debug_task.c b/云台/Application/Task/Src/debug_task.c index 2112b4e..fd23ad2 100644 --- a/云台/Application/Task/Src/debug_task.c +++ b/云台/Application/Task/Src/debug_task.c @@ -35,6 +35,7 @@ #include "attitude.h" #include "ins.h" #include "bmi088.h" +#include "gimbal.h" #include #include @@ -72,12 +73,14 @@ void debug_task(void const * argument) #if DEBUG_ATT_HEALTH int len = snprintf(line, sizeof(line), - "yaw=%.2f pitch=%.2f roll=%.2f cvg=%u rdy=%u imu=%u an=%.2f gn=%.3f%s\r\n", + "ecd=%u yaw=%.2f pitch=%.2f roll=%.2f cvg=%u rdy=%u imu=%u an=%.2f gn=%.3f%s\r\n", + (unsigned)motor_yaw_gimbal.fdb.raw_angle, att_deg_yaw, att_deg_pitch, att_deg_roll, (unsigned)INS.ConvergeFlag, (unsigned)att_ready, (unsigned)bmi088_online, an, gn, tail); #else - int len = snprintf(line, sizeof(line), "yaw=%.2f pitch=%.2f roll=%.2f\r\n", + 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); #endif diff --git a/云台/Application/Task/Src/gimbal_task.c b/云台/Application/Task/Src/gimbal_task.c index 966a725..ace0dd2 100644 --- a/云台/Application/Task/Src/gimbal_task.c +++ b/云台/Application/Task/Src/gimbal_task.c @@ -1,5 +1,5 @@ /* ============================================================================= - * gimbal_task.c - 云台控制任务(2ms) + * gimbal_task.c - 云台控制任务(2ms/500Hz) * 状态机: * YAW_WAIT 等待"姿态就绪+云台电机在线"(遥控可后到) * YAW_RUN 正常串级控制(符号用参数回退值,直至手势自检完成) @@ -26,7 +26,7 @@ #define CALIB_GESTURE_L_RL 400 // < 400 即最左 #define CALIB_GESTURE_R_RL 1600 // > 1600 即最右 #define CALIB_GESTURE_MS 500 // 保持时长 -#define CALIB_GESTURE_TICKS (CALIB_GESTURE_MS / 2) // 2ms 任务 +#define CALIB_GESTURE_TICKS (CALIB_GESTURE_MS / 2) // 2ms 任务(500Hz) typedef enum { diff --git a/云台/Application/robot_param.h b/云台/Application/robot_param.h index 92fc320..00c7ba2 100644 --- a/云台/Application/robot_param.h +++ b/云台/Application/robot_param.h @@ -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.008f // 小陀螺解算时延前馈补偿系数(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 5.5f // 输出限幅(介于慢/快陀螺之间) #define CHASSIS_FOLLOW_DEADBAND 1.0f // 误差死区(°) /* 跟随符号全自动综合:云台自检(0x1A9 flags)下发编码器 tick 增向, @@ -125,10 +128,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 1 +#define BUZZER_MELODY_SPEED_DIV 1 // 旋律降速分频(默认1:每步100ms精准匹配原版带吐音断音节拍) /* ============================ 掉线检测 ============================ */ #define DEVICE_OFFLINE_TIME_MS 100 // CAN 设备/裁判系统掉线判定时间 diff --git a/云台/MDK-ARM/Simba-ELCT-FrameWork.uvoptx b/云台/MDK-ARM/Simba-ELCT-FrameWork.uvoptx index 601c11e..e198856 100644 --- a/云台/MDK-ARM/Simba-ELCT-FrameWork.uvoptx +++ b/云台/MDK-ARM/Simba-ELCT-FrameWork.uvoptx @@ -103,7 +103,7 @@ 1 0 0 - 3 + 2 @@ -140,7 +140,7 @@ 0 CMSIS_AGDI - -X"" -O206 -S8 -C0 -P00000000 -N00("ARM CoreSight SW-DP") -D00(2BA01477) -L00(0) -TO65554 -TC10000000 -TT10000000 -TP20 -TDS8007 -TDT0 -TDC1F -TIEFFFFFFFF -TIP8 -FO15 -FD20000000 -FC1000 -FN1 -FF0STM32F4xx_1024.FLM -FS08000000 -FL0100000 -FP0($$Device:STM32F407IGHx$CMSIS\Flash\STM32F4xx_1024.FLM) + -X"Any" -UAny -O206 -S8 -C0 -P00000000 -N00("") -D00(00000000) -L00(0) -TO65554 -TC10000000 -TT10000000 -TP20 -TDS8007 -TDT0 -TDC1F -TIEFFFFFFFF -TIP8 -FO15 -FD20000000 -FC1000 -FN1 -FF0STM32F4xx_1024.FLM -FS08000000 -FL0100000 -FP0($$Device:STM32F407IGHx$CMSIS\Flash\STM32F4xx_1024.FLM) 0 @@ -1040,7 +1040,7 @@ Application/Task - 1 + 0 0 0 0 diff --git a/云台/modules/control/gimbal.c b/云台/modules/control/gimbal.c index 9e0bc15..b670844 100644 --- a/云台/modules/control/gimbal.c +++ b/云台/modules/control/gimbal.c @@ -16,6 +16,8 @@ #include "can.h" #include "robot_param.h" #include "filter.h" +#include "robot_typedef.h" +#include "dr16.h" #ifndef RAD_TO_ANGLE #define RAD_TO_ANGLE 57.295779513082320876798154814105f @@ -133,24 +135,31 @@ void gimbal_control(float yaw_delta, float pitch_delta, uint8_t reset_flag) const float yaw_meas = sign * att_deg_yaw; // ° const float yaw_rate_meas = sign * (bmi088_gyro[2] * RAD_TO_ANGLE); // °/s - /* 首次进入控制:目标角同步为当前姿态(否则 Yaw 目标仍为初始 0°, - * 一闭环就会朝 0° 猛转) */ + /* 首次进入控制或复位:将目标角设定为设定的编码器零位朝向(类似 Pitch 上电归位) */ static uint8_t first_run = 1; - if (first_run) + if (first_run || reset_flag) { first_run = 0; - yaw_target_deg = yaw_meas; - pitch_target_deg = 0; - } - if (reset_flag) - { - yaw_target_deg = yaw_meas; // Yaw 复位为当前角度 - pitch_target_deg = 0; // Pitch 复位为上电位置 - PID_PositionClean(&yaw_angle_pid); - PID_PositionClean(&yaw_speed_pid); - PID_PositionClean(&pitch_angle_pid); - PID_PositionClean(&pitch_speed_pid); + /* 计算当前编码器距离设定零位的最短差值(-4096 ~ +4096) */ + int16_t ecd_diff = (int16_t)motor_yaw_gimbal.fdb.raw_angle - (int16_t)YAW_GM6020_ENCODER_ZERO; + if (ecd_diff > 4096) + ecd_diff -= 8192; + else if (ecd_diff < -4096) + ecd_diff += 8192; + + /* 将编码器 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; + + if (reset_flag) + { + PID_PositionClean(&yaw_angle_pid); + PID_PositionClean(&yaw_speed_pid); + PID_PositionClean(&pitch_angle_pid); + PID_PositionClean(&pitch_speed_pid); + } } yaw_target_deg += yaw_delta; @@ -163,7 +172,18 @@ void gimbal_control(float yaw_delta, float pitch_delta, uint8_t reset_flag) /* Yaw 速度环:°/s -> 电压(反馈为带符号陀螺角速度,双环符号自洽) */ yaw_speed_pid.Need_Value = yaw_angle_pid.OUT; PID_PositionCalc(&yaw_speed_pid, yaw_rate_meas); - yaw_voltage = yaw_speed_pid.OUT; + + /* 小陀螺底盘自旋解耦前馈:根据底盘相对转速补偿动摩擦与反电动势拖拽，消除同向/反向手感不对称 */ + float spin_feedforward = 0.0f; + if (cmd_robot.spin_flag || dr16.s2 == 1 || dr16.s2 == 2) + { + 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; /* Pitch 串级闭环,位置环反馈为 AHRS Roll(legacy 语义) */ pitch_angle_pid.Need_Value = pitch_target_deg; diff --git a/云台/modules/dev/buzzer.c b/云台/modules/dev/buzzer.c index 59d82f6..4b28d1b 100644 --- a/云台/modules/dev/buzzer.c +++ b/云台/modules/dev/buzzer.c @@ -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]); } diff --git a/底盘/Application/Task/Src/chassis_task.c b/底盘/Application/Task/Src/chassis_task.c index 5bc4bee..03bf147 100644 --- a/底盘/Application/Task/Src/chassis_task.c +++ b/底盘/Application/Task/Src/chassis_task.c @@ -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 { diff --git a/底盘/Application/robot_param.h b/底盘/Application/robot_param.h index 92fc320..0db35a7 100644 --- a/底盘/Application/robot_param.h +++ b/底盘/Application/robot_param.h @@ -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 设备/裁判系统掉线判定时间 diff --git a/底盘/MDK-ARM/Simba-ELCT-FrameWork.uvoptx b/底盘/MDK-ARM/Simba-ELCT-FrameWork.uvoptx index 83a1fa7..528fa6a 100644 --- a/底盘/MDK-ARM/Simba-ELCT-FrameWork.uvoptx +++ b/底盘/MDK-ARM/Simba-ELCT-FrameWork.uvoptx @@ -103,7 +103,7 @@ 1 0 0 - 3 + 2 diff --git a/底盘/modules/control/chassis.c b/底盘/modules/control/chassis.c index afd7005..68cac0e 100644 --- a/底盘/modules/control/chassis.c +++ b/底盘/modules/control/chassis.c @@ -14,6 +14,7 @@ * 功率: chassis_control() 的 power_scale 直接缩放四轮输出电流 * ========================================================================== */ #include "chassis.h" +#include #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; +} + diff --git a/底盘/modules/control/chassis.h b/底盘/modules/control/chassis.h index 5b50afb..5eb9b69 100644 --- a/底盘/modules/control/chassis.h +++ b/底盘/modules/control/chassis.h @@ -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 \ No newline at end of file diff --git a/底盘/modules/dev/buzzer.c b/底盘/modules/dev/buzzer.c index 59d82f6..4b28d1b 100644 --- a/底盘/modules/dev/buzzer.c +++ b/底盘/modules/dev/buzzer.c @@ -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]); }