From dcc3b55df81cbb79cd3d14145e1f781ffa92e911 Mon Sep 17 00:00:00 2001 From: Robofish <1683502971@qq.com> Date: Thu, 9 Oct 2025 17:27:14 +0800 Subject: [PATCH] =?UTF-8?q?=E6=94=B9=E4=BB=A3=E7=A0=81?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- User/module/balance_chassis.c | 20 +- User/module/balance_chassis.h | 4 + User/module/config.c | 28 +- .../balance/parallel_legs/get_k_length.m | 4 +- .../balance/series_legs/get_k_length.m | 4 +- 未命名.jdebug | 361 ++++++++++++++++++ 6 files changed, 395 insertions(+), 26 deletions(-) create mode 100644 未命名.jdebug diff --git a/User/module/balance_chassis.c b/User/module/balance_chassis.c index fbd9577..49a8cbd 100644 --- a/User/module/balance_chassis.c +++ b/User/module/balance_chassis.c @@ -352,11 +352,11 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) { /* 构建目标状态 */ LQR_State_t target_state = {0}; - target_state.theta = 0.0f; // 目标摆杆角度 + target_state.theta = c->param->theta; // 目标摆杆角度 target_state.d_theta = 0.0f; // 目标摆杆角速度 - target_state.phi = -0.1f; // 目标俯仰角 + target_state.phi = 11.9601*pow((0.15f + c_cmd->height * 0.25f),3) + -11.8715*pow((0.15f + c_cmd->height * 0.25f),2) + 3.87083*(0.15f + c_cmd->height * 0.25f) + -0.414154; // 目标俯仰角 target_state.d_phi = 0.0f; // 目标俯仰角速度 - target_state.x = c->chassis_state.target_x; // 目标位置 + target_state.x = c->chassis_state.target_x -2.07411f*(0.15f + c_cmd->height * 0.25f) + 0.41182f; target_state.d_x = target_dot_x; // 目标速度 /* 更新LQR状态 */ @@ -384,8 +384,8 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) { } else { c->lqr[0].control_output.T = 0.0f; c->lqr[0].control_output.Tp = - c->lqr[0].K_matrix[1][6] * (-c->vmc_[0].leg.theta + 0.0f) + - c->lqr[0].K_matrix[1][7] * (-c->vmc_[0].leg.d_theta + 0.0f); + c->lqr[0].K_matrix[1][0] * (-c->vmc_[0].leg.theta + 0.0f) + + c->lqr[0].K_matrix[1][1] * (-c->vmc_[0].leg.d_theta + 0.0f); c->yaw_control.yaw_force = 0.0f; // 单腿离地时关闭偏航控制 } if (c->vmc_[1].leg.is_ground_contact) @@ -393,8 +393,8 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) { else { c->lqr[1].control_output.T = 0.0f; c->lqr[1].control_output.Tp = - c->lqr[1].K_matrix[1][6] * (-c->vmc_[1].leg.theta + 0.0f) + - c->lqr[1].K_matrix[1][7] * (-c->vmc_[1].leg.d_theta + 0.0f); + c->lqr[1].K_matrix[1][0] * (-c->vmc_[1].leg.theta + 0.0f) + + c->lqr[1].K_matrix[1][1] * (-c->vmc_[1].leg.d_theta + 0.0f); c->yaw_control.yaw_force = 0.0f; // 单腿离地时关闭偏航控制 } @@ -421,8 +421,8 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) { roll_compensation_force = -20.0f; // 目标腿长设定（移除横滚角补偿） - target_L0[0] = 0.15f + c_cmd->height * 0.2f; // 左腿：基础腿长 + 高度调节 - target_L0[1] = 0.15f + c_cmd->height * 0.2f; // 右腿：基础腿长 + 高度调节 + target_L0[0] = 0.15f + c_cmd->height * 0.25f; // 左腿：基础腿长 + 高度调节 + target_L0[1] = 0.15f + c_cmd->height * 0.25f; // 右腿：基础腿长 + 高度调节 // 获取腿长变化率 VMC_GetVirtualLegState(&c->vmc_[0], NULL, NULL, &leg_d_length[0], NULL); @@ -498,7 +498,7 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) { for (int i = 0; i < 4; i++) { if (fabsf(c->output.joint[i]) > 20.0f) { - c->output.joint[i] = (c->output.joint[i] > 0) ? 20.0f : -20.0f; + c->output.joint[i] = (c->output.joint[i] > 0) ? 25.0f : -25.0f; } } diff --git a/User/module/balance_chassis.h b/User/module/balance_chassis.h index c304bae..7178b27 100644 --- a/User/module/balance_chassis.h +++ b/User/module/balance_chassis.h @@ -91,6 +91,10 @@ typedef struct { float mech_zero_yaw; /* 机械零点 */ + float theta; + float x; + float phi; + /* 低通滤波器截止频率 */ struct { float in; /* 输入 */ diff --git a/User/module/config.c b/User/module/config.c index 4084d77..4c64f47 100644 --- a/User/module/config.c +++ b/User/module/config.c @@ -254,20 +254,24 @@ Config_RobotParam_t robot_config = { } }, .lqr_gains = { - .k11_coeff = { -1.664671105856041e+02f, 1.992942584227816e+02f, -1.128817545552375e+02f, -3.940642943456036e+00f }, // theta - .k12_coeff = { 6.755144599983973e+00f, -6.854975158723420e+00f, -7.879639603878262e+00f, -3.430989798548029e-01f }, // d_theta - .k13_coeff = { -3.418833492635837e+01f, 3.794539021170885e+01f, -1.520710246171202e+01f, -1.850485591688714e+00f }, // x - .k14_coeff = { -2.922101309139124e+01f, 3.441555363812500e+01f, -1.622627738805500e+01f, -2.575952007718017e+00f }, // d_x - .k15_coeff = { -2.999200539123322e+01f, 4.450774352569402e+01f, -2.617910446060207e+01f, 7.659044227304066e+00f }, // phi - .k16_coeff = { -6.929749592092386e+00f, 8.873396045817064e+00f, -4.631802944769120e+00f, 1.362572135878896e+00f }, // d_phi - .k21_coeff = { 1.839005962436392e+02f, -1.553482828633151e+02f, 3.110973707035321e+01f, 1.011416512244285e+01f }, // theta - .k22_coeff = { 8.875841429383772e+00f, -9.417579122119678e+00f, 3.419383245219720e+00f, 1.210428310841859e+00f }, // d_theta - .k23_coeff = { -1.702557573110695e+01f, 3.268593582331833e+01f, -2.244761887504000e+01f, 6.890533147883790e+00f }, // x - .k24_coeff = { -3.039626115318488e+01f, 4.694731664073340e+01f, -2.839718813188056e+01f, 8.384942967731277e+00f }, // d_x - .k25_coeff = { 1.199164883293124e+02f, -1.297942949573494e+02f, 5.072648972704373e+01f, 3.419795826934867e+00f }, // phi - .k26_coeff = { 1.995972391472551e+01f, -2.168740968259558e+01f, 8.589314159209088e+00f, 1.523026014539914e-01f }, // d_phi + .k11_coeff = { -1.675900301510996e+02f, 2.020661327080207e+02f, -1.145642457869424e+02f, -3.747932874943916e+00f }, // theta + .k12_coeff = { 6.610013864045129e+00f, -6.495140417893399e+00f, -8.129319798942683e+00f, -2.920993723294792e-01f }, // d_theta + .k13_coeff = { -3.558043125079351e+01f, 3.982938747687503e+01f, -1.608580664764812e+01f, -1.704388165552662e+00f }, // x + .k14_coeff = { -3.082060804555171e+01f, 3.657436465245446e+01f, -1.721864301319228e+01f, -2.425477933777937e+00f }, // d_x + .k15_coeff = { -2.915524527743928e+01f, 4.841882215140198e+01f, -3.075590139522371e+01f, 9.309579833638985e+00f }, // phi + .k16_coeff = { -6.590867790309310e+00f, 8.818728233474916e+00f, -4.823548294804866e+00f, 1.465064667201413e+00f }, // d_phi + .k21_coeff = { 2.124446115135024e+02f, -1.763726543380520e+02f, 3.279099984858797e+01f, 1.233889708835248e+01f }, // theta + .k22_coeff = { 1.207615888432813e+01f, -1.237642650067993e+01f, 4.209610950938401e+00f, 1.417825883473013e+00f }, // d_theta + .k23_coeff = { -1.482017772836761e+01f, 3.364904259474741e+01f, -2.486572037866952e+01f, 7.847915428831716e+00f }, // x + .k24_coeff = { -3.273926334829641e+01f, 5.271977032684813e+01f, -3.274024194089392e+01f, 9.730698282497507e+00f }, // d_x + .k25_coeff = { 1.663230320117893e+02f, -1.816044424207695e+02f, 7.154626742111853e+01f, 4.309975259244579e+00f }, // phi + .k26_coeff = { 2.350205493668966e+01f, -2.586342446220884e+01f, 1.038824015934221e+01f, 1.031625329478827e-01f }, // d_phi }, + .theta = 0.0f, + .x = 0.0f, + .phi = -0.1f, + .virtual_chassis_param = { .motors = { .can = BSP_CAN_1, diff --git a/utils/Simulation-master/balance/parallel_legs/get_k_length.m b/utils/Simulation-master/balance/parallel_legs/get_k_length.m index 505075b..47347c7 100644 --- a/utils/Simulation-master/balance/parallel_legs/get_k_length.m +++ b/utils/Simulation-master/balance/parallel_legs/get_k_length.m @@ -48,8 +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); - Q=diag([700 1 600 200 1000 1]);%theta d_theta x d_x phi d_phi%700 1 600 200 1000 1 - R=[240 0;0 25]; %T Tp + Q=diag(1000 1 200 200 5000 1]);%theta d_theta x d_x phi d_phi%700 1 600 200 1000 1 + R=[150 0;0 25]; %T Tp K=lqr(A,B,Q,R); 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 6dd903f..3dfc7a8 100644 --- a/utils/Simulation-master/balance/series_legs/get_k_length.m +++ b/utils/Simulation-master/balance/series_legs/get_k_length.m @@ -48,8 +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); - Q=diag([800 100 2500 1500 5000 1]);%theta d_theta x d_x phi d_phi%700 1 600 200 1000 1 - R=[140 0;0 50]; %T Tp + Q=diag([1500 100 2500 1500 8000 1]);%theta d_theta x d_x phi d_phi%700 1 600 200 1000 1 + R=[140 0;0 40]; %T Tp K=lqr(A,B,Q,R); diff --git a/未命名.jdebug b/未命名.jdebug new file mode 100644 index 0000000..a81e6a0 --- /dev/null +++ b/未命名.jdebug @@ -0,0 +1,361 @@ +/********************************************************************* +* (c) SEGGER Microcontroller GmbH * +* The Embedded Experts * +* www.segger.com * +********************************************************************** + +File : /Users/lvzucheng/Documents/R/balance_infantry/未命名.jdebug +Created : 8 Oct 2025 23:23 +Ozone Version : V3.38d +*/ + +/********************************************************************* +* +* OnProjectLoad +* +* Function description +* Project load routine. Required. +* +********************************************************************** +*/ +void OnProjectLoad (void) { + // + // Dialog-generated settings + // + Project.AddPathSubstitute ("/Users/lvzucheng/Documents/R/balance_infantry", "$(ProjectDir)"); + Project.AddPathSubstitute ("/users/lvzucheng/documents/r/balance_infantry", "$(ProjectDir)"); + Project.SetDevice ("STM32F407IG"); + Project.SetHostIF ("USB", "207400620"); + Project.SetTargetIF ("SWD"); + Project.SetTIFSpeed ("4 MHz"); + Project.AddSvdFile ("$(InstallDir)/Config/CPU/Cortex-M4F.svd"); + // + // User settings + // + File.Open ("$(ProjectDir)/build/Debug/DevC.elf"); +} + +/********************************************************************* +* +* OnStartupComplete +* +* Function description +* Called when program execution has reached/passed +* the startup completion point. Optional. +* +********************************************************************** +*/ +//void OnStartupComplete (void) { +//} + +/********************************************************************* +* +* TargetReset +* +* Function description +* Replaces the default target device reset routine. Optional. +* +* Notes +* This example demonstrates the usage when +* debugging an application in RAM on a Cortex-M target device. +* +********************************************************************** +*/ +//void TargetReset (void) { +// +// unsigned int SP; +// unsigned int PC; +// unsigned int VectorTableAddr; +// +// VectorTableAddr = Elf.GetBaseAddr(); +// // +// // Set up initial stack pointer +// // +// if (VectorTableAddr != 0xFFFFFFFF) { +// SP = Target.ReadU32(VectorTableAddr); +// Target.SetReg("SP", SP); +// } +// // +// // Set up entry point PC +// // +// PC = Elf.GetEntryPointPC(); +// +// if (PC != 0xFFFFFFFF) { +// Target.SetReg("PC", PC); +// } else if (VectorTableAddr != 0xFFFFFFFF) { +// PC = Target.ReadU32(VectorTableAddr + 4); +// Target.SetReg("PC", PC); +// } else { +// Util.Error("Project file error: failed to set entry point PC", 1); +// } +//} + +/********************************************************************* +* +* BeforeTargetReset +* +* Function description +* Event handler routine. Optional. +* +********************************************************************** +*/ +//void BeforeTargetReset (void) { +//} + +/********************************************************************* +* +* AfterTargetReset +* +* Function description +* Event handler routine. Optional. +* The default implementation initializes SP and PC to reset values. +** +********************************************************************** +*/ +void AfterTargetReset (void) { + _SetupTarget(); +} + +/********************************************************************* +* +* DebugStart +* +* Function description +* Replaces the default debug session startup routine. Optional. +* +********************************************************************** +*/ +//void DebugStart (void) { +//} + +/********************************************************************* +* +* TargetConnect +* +* Function description +* Replaces the default target IF connection routine. Optional. +* +********************************************************************** +*/ +//void TargetConnect (void) { +//} + +/********************************************************************* +* +* BeforeTargetConnect +* +* Function description +* Event handler routine. Optional. +* +********************************************************************** +*/ +//void BeforeTargetConnect (void) { +//} + +/********************************************************************* +* +* AfterTargetConnect +* +* Function description +* Event handler routine. Optional. +* +********************************************************************** +*/ +//void AfterTargetConnect (void) { +//} + +/********************************************************************* +* +* TargetDownload +* +* Function description +* Replaces the default program download routine. Optional. +* +********************************************************************** +*/ +//void TargetDownload (void) { +//} + +/********************************************************************* +* +* BeforeTargetDownload +* +* Function description +* Event handler routine. Optional. +* +********************************************************************** +*/ +//void BeforeTargetDownload (void) { +//} + +/********************************************************************* +* +* AfterTargetDownload +* +* Function description +* Event handler routine. Optional. +* The default implementation initializes SP and PC to reset values. +* +********************************************************************** +*/ +void AfterTargetDownload (void) { + _SetupTarget(); +} + +/********************************************************************* +* +* BeforeTargetDisconnect +* +* Function description +* Event handler routine. Optional. +* +********************************************************************** +*/ +//void BeforeTargetDisconnect (void) { +//} + +/********************************************************************* +* +* AfterTargetDisconnect +* +* Function description +* Event handler routine. Optional. +* +********************************************************************** +*/ +//void AfterTargetDisconnect (void) { +//} + +/********************************************************************* +* +* AfterTargetHalt +* +* Function description +* Event handler routine. Optional. +* +********************************************************************** +*/ +//void AfterTargetHalt (void) { +//} + +/********************************************************************* +* +* BeforeTargetResume +* +* Function description +* Event handler routine. Optional. +* +********************************************************************** +*/ +//void BeforeTargetResume (void) { +//} + +/********************************************************************* +* +* OnSnapshotLoad +* +* Function description +* Called upon loading a snapshot. Optional. +* +* Additional information +* This function is used to restore the target state in cases +* where values cannot simply be written to the target. +* Typical use: GPIO clock needs to be enabled, before +* GPIO is configured. +* +********************************************************************** +*/ +//void OnSnapshotLoad (void) { +//} + +/********************************************************************* +* +* OnSnapshotSave +* +* Function description +* Called upon saving a snapshot. Optional. +* +* Additional information +* This function is usually used to save values of the target +* state which can either not be trivially read, +* or need to be restored in a specific way or order. +* Typically use: Memory Mapped Registers, +* such as PLL and GPIO configuration. +* +********************************************************************** +*/ +//void OnSnapshotSave (void) { +//} + +/********************************************************************* +* +* OnError +* +* Function description +* Called when an error ocurred. Optional. +* +********************************************************************** +*/ +//void OnError (void) { +//} + +/********************************************************************* +* +* AfterProjectLoad +* +* Function description +* After Project load routine. Optional. +* +********************************************************************** +*/ +//void AfterProjectLoad (void) { +//} + +/********************************************************************* +* +* OnDebugStartBreakSymbolReached +* +* Function description +* Called when program execution has reached/passed +* the symbol to be breaked at during debug start. Optional. +* +********************************************************************** +*/ +//void OnDebugStartBreakSymReached (void) { +//} + +/********************************************************************* +* +* _SetupTarget +* +* Function description +* Setup the target. +* Called by AfterTargetReset() and AfterTargetDownload(). +* +* Auto-generated function. May be overridden by Ozone. +* +********************************************************************** +*/ +void _SetupTarget(void) { + unsigned int SP; + unsigned int PC; + unsigned int VectorTableAddr; + + VectorTableAddr = Elf.GetBaseAddr(); + // + // Set up initial stack pointer + // + SP = Target.ReadU32(VectorTableAddr); + if (SP != 0xFFFFFFFF) { + Target.SetReg("SP", SP); + } + // + // Set up entry point PC + // + PC = Elf.GetEntryPointPC(); + if (PC != 0xFFFFFFFF) { + Target.SetReg("PC", PC); + } else { + Util.Error("Project script error: failed to set up entry point PC", 1); + } +} \ No newline at end of file