robofish/rm_balance
2
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

加了个蜂鸣器

Browse Source
This commit is contained in:
Robofish 2025-10-23 01:59:27 +08:00
parent 6d9e2a184d
commit ce85f0f66c
17 changed files with 339 additions and 35 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

98
BuzzerAlarm.hpp Normal file
View File

@ -0,0 +1,98 @@
#pragma once
// clang-format off
/* === MODULE MANIFEST V2 ===
module_description: 无源蜂鸣器报警模块 / Buzzer alarm module
constructor_args:
- alarm_delay: 5000
template_args: []
required_hardware: pwm_buzzer
depends: []
=== END MANIFEST === */
// clang-format on
#include "app_framework.hpp"
#include "pwm.hpp"
class BuzzerAlarm : public LibXR::Application {
public:
// NOLINTNEXTLINE
enum class NoteName { C = 0, Cs, D, Ds, E, F, Fs, G, Gs, A, As, B };
BuzzerAlarm(LibXR::HardwareContainer& hw, LibXR::ApplicationManager& app,
uint32_t alarm_delay)
:
alarm_delay_(alarm_delay),
pwm_(hw.template FindOrExit<LibXR::PWM>({"pwm_tim12_ch2"})) {
app.Register(*this);
auto error_callback = LibXR::Assert::Callback::Create(
[](bool in_isr, BuzzerAlarm* alarm, const char* file, uint32_t line) {
UNUSED(file);
UNUSED(line);
alarm->PlayNote(NoteName::D, 4, 600);
alarm->PlayNote(NoteName::E, 4, 600);
alarm->PlayNote(NoteName::G, 4, 600);
alarm->PlayNote(NoteName::E, 4, 600);
alarm->PlayNote(NoteName::D, 4, 300);
alarm->PlayNote(NoteName::E, 4, 150);
alarm->PlayNote(NoteName::D, 4, 300);
alarm->PlayNote(NoteName::C, 4, 300);
alarm->PlayNote(NoteName::A, 3, 900);
LibXR::Thread::Sleep(300);
alarm->PlayNote(NoteName::D, 4, 600);
alarm->PlayNote(NoteName::E, 4, 600);
alarm->PlayNote(NoteName::G, 4, 600);
alarm->PlayNote(NoteName::E, 4, 600);
alarm->PlayNote(NoteName::D, 4, 300);
alarm->PlayNote(NoteName::E, 4, 150);
alarm->PlayNote(NoteName::D, 4, 300);
alarm->PlayNote(NoteName::A, 3, 300);
alarm->PlayNote(NoteName::C, 4, 900);
if (!in_isr) {
LibXR::Thread::Sleep(alarm->alarm_delay_);
}
},
this);
LibXR::Assert::RegisterFatalErrorCallback(error_callback);
PlayNote(NoteName::B, 5, 200);
PlayNote(NoteName::G, 4, 200);
PlayNote(NoteName::B, 5, 400);
PlayNote(NoteName::G, 4, 200);
PlayNote(NoteName::B, 5, 400);
PlayNote(NoteName::G, 4, 200);
PlayNote(NoteName::D, 5, 400);
PlayNote(NoteName::G, 4, 200);
PlayNote(NoteName::C, 5, 200);
PlayNote(NoteName::C, 5, 200);
PlayNote(NoteName::G, 4, 200);
PlayNote(NoteName::B, 5, 200);
PlayNote(NoteName::C, 5, 200);
}
void Play(uint32_t freq, uint32_t duration) {
pwm_->SetConfig({freq});
pwm_->Enable();
pwm_->SetDutyCycle(0.5);
LibXR::Thread::Sleep(duration);
pwm_->Disable();
}
void PlayNote(NoteName note, uint32_t octave, uint32_t duration) {
int midi_num = static_cast<int>(note) + static_cast<int>((octave + 1) * 12);
float freq =
440.0f * std::pow(2.0f, (static_cast<float>(midi_num) - 69.0f) / 12.0f);
Play(static_cast<uint32_t>(freq), static_cast<uint32_t>(duration));
}
void OnMonitor() override {
}
private:
uint32_t alarm_delay_;
LibXR::PWM* pwm_;
};

1
CMakeLists.txt
View File

@ -98,6 +98,7 @@ target_sources(${CMAKE_PROJECT_NAME} PRIVATE
User/task/ctrl_shoot.c
User/task/init.c
User/task/monitor.c
User/task/music.c
User/task/rc.c
User/task/user_task.c
)

3
User/device/ai.c
View File

@ -32,6 +32,8 @@ static osThreadId_t thread_alert;
static uint32_t drop_message = 0;
// uint16_t crc16;
/* Private function -------------------------------------------------------- */
static void Ai_RxCpltCallback(void) {
@ -80,6 +82,7 @@ bool AI_WaitDmaCplt(void) {
}
int8_t AI_ParseHost(AI_t *ai) {
// crc16 = CRC16_Calc((const uint8_t *)&(rxbuf), sizeof(ai->from_host) - 2, CRC16_INIT);
if (!CRC16_Verify((const uint8_t *)&(rxbuf), sizeof(ai->from_host)))
goto error;
ai->header.online = true;

4
User/device/ai.h
View File

@ -62,8 +62,10 @@ typedef struct __packed {
/* 电控 -> 视觉 裁判系统数据结构体*/
typedef struct __packed {
/* USER REFEREE BEGIN */
uint16_t team; /* 本身队伍 */
uint16_t time; /* 比赛开始时间 */
/* USER REFEREE END */
} AI_Protocol_UpDataReferee_t;
/* 视觉 -> 电控 数据包结构体*/

119
User/device/buzzer.c
View File

@ -1,4 +1,6 @@
#include "device/buzzer.h"
#include "bsp/time.h"
#include <math.h>
/* USER INCLUDE BEGIN */
@ -8,6 +10,83 @@
/* USER DEFINE END */
#define MUSIC_DEFAULT_VOLUME 0.5f
#define MUSIC_A4_FREQ 440.0f // A4音符频率
const Tone_t RM[] = {
{NOTE_B, 5, 200},
{NOTE_G, 4, 200},
{NOTE_B, 5, 400},
{NOTE_G, 4, 200},
{NOTE_B, 5, 400},
{NOTE_G, 4, 200},
{NOTE_D, 5, 400},
{NOTE_G, 4, 200},
{NOTE_C, 5, 200},
{NOTE_C, 5, 200},
{NOTE_G, 4, 200},
{NOTE_B, 5, 200},
{NOTE_C, 5, 200}
};
// Nokia 经典铃声音符
const Tone_t NOKIA[] = {
{NOTE_E, 5, 125}, {NOTE_D, 5, 125}, {NOTE_FS, 4, 250}, {NOTE_GS, 4, 250},
{NOTE_CS, 5, 125}, {NOTE_B, 4, 125}, {NOTE_D, 4, 250}, {NOTE_E, 4, 250},
{NOTE_B, 4, 125}, {NOTE_A, 4, 125}, {NOTE_CS, 4, 250}, {NOTE_E, 4, 250},
{NOTE_A, 4, 500}
};
static void BUZZER_Update(BUZZER_t *buzzer){
buzzer->header.online = true;
buzzer->header.last_online_time = BSP_TIME_Get_ms();
}
// 根据音符和八度计算频率的辅助函数
static float BUZZER_CalcFreq(NOTE_t note, uint8_t octave) {
if (note == NOTE_REST) {
return 0.0f; // 休止符返回0频率
}
// 将音符和八度转换为MIDI音符编号
int midi_num = (int)note + (int)((octave + 1) * 12);
// 使用A4 (440Hz) 作为参考，计算频率
// 公式: freq = 440 * 2^((midi_num - 69)/12)
float freq = 440.0f * powf(2.0f, ((float)midi_num - 69.0f) / 12.0f);
return freq;
}
// 播放单个音符
static int8_t BUZZER_PlayTone(BUZZER_t *buzzer, NOTE_t note, uint8_t octave, uint16_t duration_ms) {
if (buzzer == NULL || !buzzer->header.online)
return DEVICE_ERR;
float freq = BUZZER_CalcFreq(note, octave);
if (freq > 0.0f) {
// 播放音符
if (BUZZER_Set(buzzer, freq, MUSIC_DEFAULT_VOLUME) != DEVICE_OK)
return DEVICE_ERR;
if (BUZZER_Start(buzzer) != DEVICE_OK)
return DEVICE_ERR;
} else {
// 休止符，停止播放
BUZZER_Stop(buzzer);
}
// 等待指定时间
BSP_TIME_Delay_ms(duration_ms);
// 停止当前音符，为下一个音符做准备
BUZZER_Stop(buzzer);
BSP_TIME_Delay_ms(20); // 短暂间隔
return DEVICE_OK;
}
int8_t BUZZER_Init(BUZZER_t *buzzer, BSP_PWM_Channel_t channel) {
if (buzzer == NULL) return DEVICE_ERR;
@ -23,7 +102,7 @@ int8_t BUZZER_Init(BUZZER_t *buzzer, BSP_PWM_Channel_t channel) {
int8_t BUZZER_Start(BUZZER_t *buzzer) {
if (buzzer == NULL || !buzzer->header.online)
return DEVICE_ERR;
BUZZER_Update(buzzer);
return (BSP_PWM_Start(buzzer->channel) == BSP_OK) ?
DEVICE_OK : DEVICE_ERR;
}
@ -31,7 +110,7 @@ int8_t BUZZER_Start(BUZZER_t *buzzer) {
int8_t BUZZER_Stop(BUZZER_t *buzzer) {
if (buzzer == NULL || !buzzer->header.online)
return DEVICE_ERR;
BUZZER_Update(buzzer);
return (BSP_PWM_Stop(buzzer->channel) == BSP_OK) ?
DEVICE_OK : DEVICE_ERR;
}
@ -41,7 +120,7 @@ int8_t BUZZER_Set(BUZZER_t *buzzer, float freq, float duty_cycle) {
return DEVICE_ERR;
int result = DEVICE_OK ;
BUZZER_Update(buzzer);
if (BSP_PWM_SetFreq(buzzer->channel, freq) != BSP_OK)
result = DEVICE_ERR;
@ -51,6 +130,40 @@ int8_t BUZZER_Set(BUZZER_t *buzzer, float freq, float duty_cycle) {
return result;
}
int8_t BUZZER_PlayMusic(BUZZER_t *buzzer, MUSIC_t music) {
if (buzzer == NULL || !buzzer->header.online)
return DEVICE_ERR;
const Tone_t *melody = NULL;
size_t melody_length = 0;
// 根据音乐类型选择对应的音符数组
switch (music) {
case MUSIC_RM:
melody = RM;
melody_length = sizeof(RM) / sizeof(Tone_t);
break;
case MUSIC_NOKIA:
melody = NOKIA;
melody_length = sizeof(NOKIA) / sizeof(Tone_t);
break;
default:
return DEVICE_ERR;
}
// 播放整首音乐
for (size_t i = 0; i < melody_length; i++) {
if (BUZZER_PlayTone(buzzer, melody[i].note, melody[i].octave, melody[i].duration_ms) != DEVICE_OK) {
BUZZER_Stop(buzzer); // 出错时停止播放
return DEVICE_ERR;
}
}
// 音乐播放完成后停止
BUZZER_Stop(buzzer);
return DEVICE_OK;
}
/* USER FUNCTION BEGIN */
/* USER FUNCTION END */

35
User/device/buzzer.h
View File

@ -8,6 +8,7 @@ extern "C" {
#include "device.h"
#include "bsp/pwm.h"
#include <stddef.h>
#include <stdint.h>
/* USER INCLUDE BEGIN */
@ -20,6 +21,35 @@ extern "C" {
/* USER DEFINE END */
/* Exported types ----------------------------------------------------------- */
typedef enum {
NOTE_C = 0,
NOTE_CS = 1, // C#
NOTE_D = 2,
NOTE_DS = 3, // D#
NOTE_E = 4,
NOTE_F = 5,
NOTE_FS = 6, // F#
NOTE_G = 7,
NOTE_GS = 8, // G#
NOTE_A = 9,
NOTE_AS = 10, // A#
NOTE_B = 11,
NOTE_REST = 255 // 休止符
} NOTE_t;
typedef struct {
NOTE_t note; // 音符名称
uint8_t octave; // 八度
uint16_t duration_ms; // 持续时间，单位毫秒
} Tone_t;
typedef enum {
MUSIC_RM,
MUSIC_NOKIA,
} MUSIC_t;
typedef struct {
DEVICE_Header_t header;
BSP_PWM_Channel_t channel;
@ -33,15 +63,14 @@ typedef struct {
int8_t BUZZER_Init(BUZZER_t *buzzer, BSP_PWM_Channel_t channel);
int8_t BUZZER_Start(BUZZER_t *buzzer);
int8_t BUZZER_Stop(BUZZER_t *buzzer);
int8_t BUZZER_Set(BUZZER_t *buzzer, float freq, float duty_cycle);
int8_t BUZZER_PlayMusic(BUZZER_t *buzzer, MUSIC_t music);
/* USER FUNCTION BEGIN */
/* USER FUNCTION END */

5
User/device/device_config.yaml
View File

@ -1,3 +1,6 @@
ai:
bsp_config: {}
enabled: true
bmi088:
bsp_config:
BSP_GPIO_ACCL_CS: BSP_GPIO_ACCL_CS
@ -8,7 +11,7 @@ bmi088:
enabled: true
buzzer:
bsp_config:
BSP_PWM_BUZZER: BSP_PWM_TIM8_CH1
BSP_PWM_BUZZER: BSP_PWM_BUZZER
enabled: true
dm_imu:
bsp_config: {}

28
User/module/config.c
View File

@ -254,20 +254,20 @@ Config_RobotParam_t robot_config = {
}
},
.lqr_gains = {
.k11_coeff = { -2.213202553185133e+02f, 2.353939463356143e+02f, -1.057072351438971e+02f, -1.581085937677281e+00f }, // theta
.k12_coeff = { -9.181864404672975e+00f, 8.531964722737065e+00f, -9.696625432480346e+00f, -2.388898921230960e-01f }, // d_theta
.k13_coeff = { -6.328339397527442e+01f, 6.270159865929592e+01f, -2.133356351416681e+01f, -2.795774497769496e-01f }, // x
.k14_coeff = { -7.428160824353201e+01f, 7.371925049068537e+01f, -2.613745545093503e+01f, -5.994101373770330e-01f }, // d_x
.k15_coeff = { -6.968934105907989e+01f, 9.229969229361623e+01f, -4.424018428098277e+01f, 8.098181536555296e+00f }, // phi
.k16_coeff = { -1.527045508038401e+01f, 2.030548630730375e+01f, -1.009526207086012e+01f, 2.014358176738665e+00f }, // d_phi
.k21_coeff = { 6.254476937997669e+01f, 9.037146968574660e+00f, -4.492072460618583e+01f, 1.770766202994207e+01f }, // theta
.k22_coeff = { 3.165057029795604e-02f, 7.350960766534424e+00f, -6.597366624137901e+00f, 2.798506180182324e+00f }, // d_theta
.k23_coeff = { -5.827814614802593e+01f, 7.789995488757775e+01f, -3.841148024725668e+01f, 8.034534049078013e+00f }, // x
.k24_coeff = { -8.937952443465080e+01f, 1.128943502182752e+02f, -5.293642666103645e+01f, 1.073722383888271e+01f }, // d_x
.k25_coeff = { 2.478483065877546e+02f, -2.463640234149189e+02f, 8.359617215530402e+01f, 8.324247402653134e+00f }, // phi
.k26_coeff = { 6.307211927250707e+01f, -6.266313408748906e+01f, 2.129449351279647e+01f, 9.249265186231070e-01f }, // d_phi
},
.lqr_gains = {
.k11_coeff = { -3.147362972581901e+02f, 3.207074456637030e+02f, -1.299720619449884e+02f, -3.569397240304915e+00f }, // theta
.k12_coeff = { -1.586071559868343e+01f, 1.449431164705369e+01f, -1.121221716719879e+01f, -3.451792762764330e-01f }, // d_theta
.k13_coeff = { -5.618790901635980e+01f, 5.379369275290406e+01f, -1.745182841455727e+01f, -6.761399950784669e-01f }, // x
.k14_coeff = { -7.173250530567481e+01f, 6.856776466550843e+01f, -2.301072504290219e+01f, -1.099155058815247e+00f }, // d_x
.k15_coeff = { -9.613460546380958e+01f, 1.048862571784616e+02f, -4.025218968092935e+01f, 5.321247456640327e+00f }, // phi
.k16_coeff = { -7.052289754532913e+01f, 7.982089604487435e+01f, -3.307700050901695e+01f, 5.645737364902630e+00f }, // d_phi
.k21_coeff = { -1.057133693867297e+02f, 1.725695883025699e+02f, -9.737528883036143e+01f, 2.163196664625316e+01f }, // theta
.k22_coeff = { -2.315951596370687e+01f, 2.955906799408467e+01f, -1.354452600882797e+01f, 3.321564141904335e+00f }, // d_theta
.k23_coeff = { -8.976880402662755e+01f, 1.014166193118393e+02f, -4.187310761184219e+01f, 7.291792646751865e+00f }, // x
.k24_coeff = { -1.341531876405906e+02f, 1.488885000296862e+02f, -6.019345895548503e+01f, 1.030125371802067e+01f }, // d_x
.k25_coeff = { 1.384441101996959e+02f, -1.307641033174926e+02f, 4.125133391735927e+01f, 1.109615083184617e+01f }, // phi
.k26_coeff = { 1.826205273539437e+02f, -1.750288341017617e+02f, 5.676978575378812e+01f, 3.699479836742944e+00f }, // d_phi
},
.theta = 0.0f,
.x = 0.0f,
.phi = -0.1f,

1
User/task/ai.c
View File

@ -4,7 +4,6 @@
*/
/* Includes ----------------------------------------------------------------- */
#include "cmsis_os2.h"
#include "task/user_task.h"
/* USER INCLUDE BEGIN */
#include "device/ai.h"

1
User/task/atti_esti.c
View File

@ -4,7 +4,6 @@
*/
/* Includes ----------------------------------------------------------------- */
#include "cmsis_os2.h"
#include "task/user_task.h"
/* USER INCLUDE BEGIN */
#include "bsp/mm.h"

6
User/task/config.yaml
View File

@ -61,3 +61,9 @@
function: Task_cmd
name: cmd
stack: 256
- delay: 0
description: ''
freq_control: false
function: Task_music
name: music
stack: 256

1
User/task/init.c
View File

@ -42,6 +42,7 @@ void Task_Init(void *argument) {
task_runtime.thread.monitor = osThreadNew(Task_monitor, NULL, &attr_monitor);
task_runtime.thread.ai = osThreadNew(Task_ai, NULL, &attr_ai);
task_runtime.thread.cmd = osThreadNew(Task_cmd, NULL, &attr_cmd);
task_runtime.thread.music = osThreadNew(Task_music, NULL, &attr_music);
// 创建消息队列
/* USER MESSAGE BEGIN */

44
User/task/music.c Normal file
View File

@ -0,0 +1,44 @@
/*
music Task
*/
/* Includes ----------------------------------------------------------------- */
#include "bsp/time.h"
#include "task/user_task.h"
/* USER INCLUDE BEGIN */
#include "device/buzzer.h"
#include "bsp/pwm.h"
/* USER INCLUDE END */
/* Private typedef ---------------------------------------------------------- */
/* Private define ----------------------------------------------------------- */
/* Private macro ------------------------------------------------------------ */
/* Private variables -------------------------------------------------------- */
/* USER STRUCT BEGIN */
BUZZER_t buzzer;
/* USER STRUCT END */
/* Private function --------------------------------------------------------- */
/* Exported functions ------------------------------------------------------- */
void Task_music(void *argument) {
(void)argument; /* 未使用argument，消除警告 */
osDelay(MUSIC_INIT_DELAY); /* 延时一段时间再开启任务 */
/* USER CODE INIT BEGIN */
BUZZER_Init(&buzzer, BSP_PWM_BUZZER);
/* USER CODE INIT END */
while (1) {
/* USER CODE BEGIN */
// BUZZER_PlayMusic(&buzzer, MUSIC_RM);
// BUZZER_Init(&buzzer, BSP_PWM_BUZZER);
BSP_TIME_Delay_ms(1000); // 等待音乐播放完成
/* USER CODE END */
}
}

8
User/task/rc.c
View File

@ -61,13 +61,13 @@ void Task_rc(void *argument) {
cmd_for_chassis.mode = CHASSIS_MODE_RELAX;
break;
case DR16_SW_MID:
// cmd_for_chassis.mode = CHASSIS_MODE_RECOVER;
cmd_for_chassis.mode = CHASSIS_MODE_WHELL_LEG_BALANCE;
cmd_for_chassis.mode = CHASSIS_MODE_RECOVER;
// cmd_for_chassis.mode = CHASSIS_MODE_WHELL_LEG_BALANCE;
break;
case DR16_SW_DOWN:
// cmd_for_chassis.mode = CHASSIS_MODE_ROTOR;
// cmd_for_chassis.mode = CHASSIS_MODE_WHELL_LEG_BALANCE;
cmd_for_chassis.mode = CHASSIS_MODE_JUMP;
cmd_for_chassis.mode = CHASSIS_MODE_WHELL_LEG_BALANCE;
// cmd_for_chassis.mode = CHASSIS_MODE_JUMP;
break;
default:
cmd_for_chassis.mode = CHASSIS_MODE_RELAX;

5
User/task/user_task.c
View File

@ -53,4 +53,9 @@ const osThreadAttr_t attr_cmd = {
.name = "cmd",
.priority = osPriorityNormal,
.stack_size = 256 * 4,
};
const osThreadAttr_t attr_music = {
.name = "music",
.priority = osPriorityNormal,
.stack_size = 256 * 4,
};

5
User/task/user_task.h
View File

@ -34,6 +34,7 @@ extern "C" {
#define MONITOR_INIT_DELAY (0)
#define AI_INIT_DELAY (0)
#define CMD_INIT_DELAY (0)
#define MUSIC_INIT_DELAY (0)
/* Exported defines --------------------------------------------------------- */
/* Exported macro ----------------------------------------------------------- */
@ -52,6 +53,7 @@ typedef struct {
osThreadId_t monitor;
osThreadId_t ai;
osThreadId_t cmd;
osThreadId_t music;
} thread;
/* USER MESSAGE BEGIN */
@ -106,6 +108,7 @@ typedef struct {
UBaseType_t monitor;
UBaseType_t ai;
UBaseType_t cmd;
UBaseType_t music;
} stack_water_mark;
/* 各任务运行频率 */
@ -150,6 +153,7 @@ extern const osThreadAttr_t attr_ctrl_shoot;
extern const osThreadAttr_t attr_monitor;
extern const osThreadAttr_t attr_ai;
extern const osThreadAttr_t attr_cmd;
extern const osThreadAttr_t attr_music;
/* 任务函数声明 */
void Task_Init(void *argument);
@ -162,6 +166,7 @@ void Task_ctrl_shoot(void *argument);
void Task_monitor(void *argument);
void Task_ai(void *argument);
void Task_cmd(void *argument);
void Task_music(void *argument);
#ifdef __cplusplus
}

10
utils/Simulation-master/balance/series_legs/get_k_length.m
View File

@ -20,7 +20,7 @@ function K = get_k_length(leg_length)
l1=-0.03; %机体质心距离转轴距离
mw1=0.60; %驱动轮质量
mp1=1.8; %杆质量
M1=6.0; %机体质量
M1=10.0; %机体质量
Iw1=mw1*R1^2; %驱动轮转动惯量
Ip1=mp1*((L1+LM1)^2+0.05^2)/12.0; %摆杆转动惯量
IM1=M1*(0.3^2+0.12^2)/12.0; %机体绕质心转动惯量
@ -48,12 +48,8 @@ function K = get_k_length(leg_length)
B=subs(B,[R,L,LM,l,mw,mp,M,Iw,Ip,IM,g],[R1,L1,LM1,l1,mw1,mp1,M1,Iw1,Ip1,IM1,9.8]);
B=double(B);
% Recommended: Increase velocity damping to improve convergence
% Q weights [theta, d_theta, x, d_x, phi, d_phi]
% Original: [1000, 100, 2000, 1500, 20000, 500] causes self-oscillation
% Improved: [500, 300, 2000, 1500, 20000, 500] weight ratio 1.67:1
Q=diag([600 300 2000 1500 20000 500]);
R=[240 0;0 50];
Q=diag([600 450 2000 1500 20000 6000]);%theta d_theta x d_x phi d_phi%700 1 600 200 1000 1
R=[280 0;0 60]; %T Tp
K=lqr(A,B,Q,R);