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
云台/modules/control/gimbal.c
+35 -15
View File
@@ -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;
云台/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]);
}