From ce85f0f66cd2afc9509ee79c05ab74b19bf797da Mon Sep 17 00:00:00 2001 From: Robofish <1683502971@qq.com> Date: Thu, 23 Oct 2025 01:59:27 +0800 Subject: [PATCH] =?UTF-8?q?=E5=8A=A0=E4=BA=86=E4=B8=AA=E8=9C=82=E9=B8=A3?= =?UTF-8?q?=E5=99=A8?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- BuzzerAlarm.hpp | 98 +++++++++++++++ CMakeLists.txt | 1 + User/device/ai.c | 3 + User/device/ai.h | 4 +- User/device/buzzer.c | 119 +++++++++++++++++- User/device/buzzer.h | 35 +++++- User/device/device_config.yaml | 5 +- User/module/config.c | 28 ++--- User/task/ai.c | 1 - User/task/atti_esti.c | 1 - User/task/config.yaml | 6 + User/task/init.c | 1 + User/task/music.c | 44 +++++++ User/task/rc.c | 8 +- User/task/user_task.c | 5 + User/task/user_task.h | 5 + .../balance/series_legs/get_k_length.m | 10 +- 17 files changed, 339 insertions(+), 35 deletions(-) create mode 100644 BuzzerAlarm.hpp create mode 100644 User/task/music.c diff --git a/BuzzerAlarm.hpp b/BuzzerAlarm.hpp new file mode 100644 index 0000000..d458c20 --- /dev/null +++ b/BuzzerAlarm.hpp @@ -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({"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(note) + static_cast((octave + 1) * 12); + float freq = + 440.0f * std::pow(2.0f, (static_cast(midi_num) - 69.0f) / 12.0f); + Play(static_cast(freq), static_cast(duration)); + } + + void OnMonitor() override { + } + +private: + uint32_t alarm_delay_; + + LibXR::PWM* pwm_; +}; \ No newline at end of file diff --git a/CMakeLists.txt b/CMakeLists.txt index 30c0629..b903dd6 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -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 ) diff --git a/User/device/ai.c b/User/device/ai.c index 9bc3f88..2632536 100644 --- a/User/device/ai.c +++ b/User/device/ai.c @@ -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; diff --git a/User/device/ai.h b/User/device/ai.h index 2ce35ca..47841e0 100644 --- a/User/device/ai.h +++ b/User/device/ai.h @@ -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; /* 视觉 -> 电控 数据包结构体*/ diff --git a/User/device/buzzer.c b/User/device/buzzer.c index 1996348..bf04db5 100644 --- a/User/device/buzzer.c +++ b/User/device/buzzer.c @@ -1,4 +1,6 @@ #include "device/buzzer.h" +#include "bsp/time.h" +#include /* 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 */ diff --git a/User/device/buzzer.h b/User/device/buzzer.h index 8b1246e..e2deeb8 100644 --- a/User/device/buzzer.h +++ b/User/device/buzzer.h @@ -8,6 +8,7 @@ extern "C" { #include "device.h" #include "bsp/pwm.h" #include +#include /* 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 */ diff --git a/User/device/device_config.yaml b/User/device/device_config.yaml index c48634b..5c92563 100644 --- a/User/device/device_config.yaml +++ b/User/device/device_config.yaml @@ -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: {} diff --git a/User/module/config.c b/User/module/config.c index 686c605..285636e 100644 --- a/User/module/config.c +++ b/User/module/config.c @@ -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, diff --git a/User/task/ai.c b/User/task/ai.c index c72f9c8..2bed1d9 100644 --- a/User/task/ai.c +++ b/User/task/ai.c @@ -4,7 +4,6 @@ */ /* Includes ----------------------------------------------------------------- */ -#include "cmsis_os2.h" #include "task/user_task.h" /* USER INCLUDE BEGIN */ #include "device/ai.h" diff --git a/User/task/atti_esti.c b/User/task/atti_esti.c index 15ddb5c..dbc5e25 100644 --- a/User/task/atti_esti.c +++ b/User/task/atti_esti.c @@ -4,7 +4,6 @@ */ /* Includes ----------------------------------------------------------------- */ -#include "cmsis_os2.h" #include "task/user_task.h" /* USER INCLUDE BEGIN */ #include "bsp/mm.h" diff --git a/User/task/config.yaml b/User/task/config.yaml index 0104238..1c903f1 100644 --- a/User/task/config.yaml +++ b/User/task/config.yaml @@ -61,3 +61,9 @@ function: Task_cmd name: cmd stack: 256 +- delay: 0 + description: '' + freq_control: false + function: Task_music + name: music + stack: 256 diff --git a/User/task/init.c b/User/task/init.c index ae40091..cdb86d9 100644 --- a/User/task/init.c +++ b/User/task/init.c @@ -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 */ diff --git a/User/task/music.c b/User/task/music.c new file mode 100644 index 0000000..6841487 --- /dev/null +++ b/User/task/music.c @@ -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 */ + } + +} \ No newline at end of file diff --git a/User/task/rc.c b/User/task/rc.c index 3a8d9b8..d913414 100644 --- a/User/task/rc.c +++ b/User/task/rc.c @@ -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; diff --git a/User/task/user_task.c b/User/task/user_task.c index d3ddc0a..433abec 100644 --- a/User/task/user_task.c +++ b/User/task/user_task.c @@ -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, }; \ No newline at end of file diff --git a/User/task/user_task.h b/User/task/user_task.h index 53eeb03..f01fb4e 100644 --- a/User/task/user_task.h +++ b/User/task/user_task.h @@ -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 } diff --git a/utils/Simulation-master/balance/series_legs/get_k_length.m b/utils/Simulation-master/balance/series_legs/get_k_length.m index 647014a..3464669 100644 --- a/utils/Simulation-master/balance/series_legs/get_k_length.m +++ b/utils/Simulation-master/balance/series_legs/get_k_length.m @@ -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);