diff --git a/User/device/buzzer.c b/User/device/buzzer.c index bf04db5..f5cc820 100644 --- a/User/device/buzzer.c +++ b/User/device/buzzer.c @@ -13,7 +13,8 @@ #define MUSIC_DEFAULT_VOLUME 0.5f #define MUSIC_A4_FREQ 440.0f // A4音符频率 - +/* USER MUSIC MENU BEGIN */ +// RM音乐 const Tone_t RM[] = { {NOTE_B, 5, 200}, {NOTE_G, 4, 200}, @@ -37,6 +38,7 @@ const Tone_t NOKIA[] = { {NOTE_B, 4, 125}, {NOTE_A, 4, 125}, {NOTE_CS, 4, 250}, {NOTE_E, 4, 250}, {NOTE_A, 4, 500} }; +/* USER MUSIC MENU END */ static void BUZZER_Update(BUZZER_t *buzzer){ buzzer->header.online = true; diff --git a/User/device/buzzer.h b/User/device/buzzer.h index e2deeb8..4ef7e43 100644 --- a/User/device/buzzer.h +++ b/User/device/buzzer.h @@ -1,3 +1,11 @@ +/** + * @file buzzer.h + * @brief 蜂鸣器设备驱动头文件 + * @details 提供蜂鸣器音频播放功能，支持单音符播放和预设音乐播放 + * @author Generated by STM32CubeMX + * @date 2025年10月23日 + */ + #pragma once #ifdef __cplusplus @@ -5,10 +13,10 @@ extern "C" { #endif /* Includes ----------------------------------------------------------------- */ -#include "device.h" -#include "bsp/pwm.h" -#include -#include +#include "bsp/pwm.h" // PWM底层硬件抽象层 +#include "device.h" // 设备通用头文件 +#include // 标准定义 +#include // 标准整型定义 /* USER INCLUDE BEGIN */ @@ -22,37 +30,54 @@ extern "C" { /* Exported types ----------------------------------------------------------- */ +/** + * @brief 音符枚举类型 + * @details 定义标准十二平均律音符，用于音乐播放 + */ 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_C = 0, ///< Do音符 + NOTE_CS = 1, ///< Do#音符 (升Do) + NOTE_D = 2, ///< Re音符 + NOTE_DS = 3, ///< Re#音符 (升Re) + NOTE_E = 4, ///< Mi音符 + NOTE_F = 5, ///< Fa音符 + NOTE_FS = 6, ///< Fa#音符 (升Fa) + NOTE_G = 7, ///< Sol音符 + NOTE_GS = 8, ///< Sol#音符 (升Sol) + NOTE_A = 9, ///< La音符 + NOTE_AS = 10, ///< La#音符 (升La) + NOTE_B = 11, ///< Si音符 + NOTE_REST = 255 ///< 休止符 (无声音) } NOTE_t; +/** + * @brief 音调结构体 + * @details 定义一个完整的音调信息，包括音符、八度和持续时间 + */ typedef struct { - NOTE_t note; // 音符名称 - uint8_t octave; // 八度 - uint16_t duration_ms; // 持续时间，单位毫秒 + NOTE_t note; ///< 音符名称 (使用NOTE_t枚举) + uint8_t octave; ///< 八度 (0-8，通常使用3-7) + uint16_t duration_ms; ///< 持续时间，单位毫秒 } Tone_t; +/** + * @brief 预设音乐枚举类型 + * @details 定义可播放的预设音乐类型 + */ typedef enum { - MUSIC_RM, - MUSIC_NOKIA, + /* USER MUSIC MENU BEGIN */ + MUSIC_RM, ///< RM战队音乐 + MUSIC_NOKIA, ///< 诺基亚经典铃声 + /* USER MUSIC MENU END */ } MUSIC_t; - +/** + * @brief 蜂鸣器设备结构体 + * @details 蜂鸣器设备的完整描述，包含设备头信息和PWM通道 + */ typedef struct { - DEVICE_Header_t header; - BSP_PWM_Channel_t channel; + DEVICE_Header_t header; ///< 设备通用头信息 (在线状态、时间戳等) + BSP_PWM_Channel_t channel; ///< PWM输出通道 } BUZZER_t; /* USER STRUCT BEGIN */ @@ -61,14 +86,47 @@ typedef struct { /* Exported functions prototypes -------------------------------------------- */ +/** + * @brief 初始化蜂鸣器设备 + * @param buzzer 蜂鸣器设备结构体指针 + * @param channel PWM输出通道 + * @return int8_t 返回值：DEVICE_OK(0) 成功，DEVICE_ERR(-1) 失败 + * @note 初始化后蜂鸣器处于停止状态 + */ int8_t BUZZER_Init(BUZZER_t *buzzer, BSP_PWM_Channel_t channel); +/** + * @brief 启动蜂鸣器播放 + * @param buzzer 蜂鸣器设备结构体指针 + * @return int8_t 返回值：DEVICE_OK(0) 成功，DEVICE_ERR(-1) 失败 + * @note 需要先调用BUZZER_Set设置频率和占空比 + */ int8_t BUZZER_Start(BUZZER_t *buzzer); +/** + * @brief 停止蜂鸣器播放 + * @param buzzer 蜂鸣器设备结构体指针 + * @return int8_t 返回值：DEVICE_OK(0) 成功，DEVICE_ERR(-1) 失败 + */ int8_t BUZZER_Stop(BUZZER_t *buzzer); +/** + * @brief 设置蜂鸣器频率和占空比 + * @param buzzer 蜂鸣器设备结构体指针 + * @param freq 频率 (Hz)，通常范围20Hz-20kHz + * @param duty_cycle 占空比 (0.0-1.0)，影响音量大小 + * @return int8_t 返回值：DEVICE_OK(0) 成功，DEVICE_ERR(-1) 失败 + * @note 设置后需要调用BUZZER_Start才能听到声音 + */ int8_t BUZZER_Set(BUZZER_t *buzzer, float freq, float duty_cycle); +/** + * @brief 播放预设音乐 + * @param buzzer 蜂鸣器设备结构体指针 + * @param music 音乐类型 (使用MUSIC_t枚举) + * @return int8_t 返回值：DEVICE_OK(0) 成功，DEVICE_ERR(-1) 失败 + * @note 这是一个阻塞函数，会播放完整首音乐后返回 + */ int8_t BUZZER_PlayMusic(BUZZER_t *buzzer, MUSIC_t music); /* USER FUNCTION BEGIN */ diff --git a/User/module/config.c b/User/module/config.c index 285636e..66ecc42 100644 --- a/User/module/config.c +++ b/User/module/config.c @@ -254,19 +254,20 @@ Config_RobotParam_t robot_config = { } }, + .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 + .k11_coeff = { -2.299492214443364e+02f, 2.371048664025156e+02f, -1.091592921067477e+02f, -5.004256900558796e+00f }, // theta + .k12_coeff = { -5.160496889728024e+00f, 3.062519293243873e+00f, -9.339456933350174e+00f, -4.793280770575950e-01f }, // d_theta + .k13_coeff = { -5.187383520564029e+01f, 4.899697460241380e+01f, -1.565478588426995e+01f, -2.004694192093775e+00f }, // x + .k14_coeff = { -5.668796498905875e+01f, 5.377351750824888e+01f, -1.871718521370502e+01f, -2.691170008421150e+00f }, // d_x + .k15_coeff = { -1.449457451875909e+02f, 1.491544007677516e+02f, -5.400783326406858e+01f, 7.212250094701016e+00f }, // phi + .k16_coeff = { -9.623232912225313e+01f, 1.012699482439727e+02f, -3.862842162301360e+01f, 6.174165209610019e+00f }, // d_phi + .k21_coeff = { -1.197720213257983e+02f, 1.432558800683586e+02f, -6.332976803783767e+01f, 1.244403075562354e+01f }, // theta + .k22_coeff = { -1.641975451035667e+01f, 1.816373049685376e+01f, -6.825602612420409e+00f, 1.759808519224317e+00f }, // d_theta + .k23_coeff = { -9.525505626411980e+01f, 9.986545276773818e+01f, -3.780741158172127e+01f, 6.150010121224194e+00f }, // x + .k24_coeff = { -1.249520841685426e+02f, 1.297949868891588e+02f, -4.847618771898694e+01f, 7.842551207050604e+00f }, // d_x + .k25_coeff = { 1.098971996183004e+02f, -1.028972351169783e+02f, 3.234322461758097e+01f, 1.250515887186789e+01f }, // phi + .k26_coeff = { 1.122150774085550e+02f, -1.059683417765082e+02f, 3.379315768085248e+01f, 5.474171975795353e+00f }, // d_phi }, .theta = 0.0f, .x = 0.0f, diff --git a/User/task/music.c b/User/task/music.c index 6841487..b5f6d8c 100644 --- a/User/task/music.c +++ b/User/task/music.c @@ -29,15 +29,12 @@ void Task_music(void *argument) { /* USER CODE INIT BEGIN */ BUZZER_Init(&buzzer, BSP_PWM_BUZZER); - + BUZZER_PlayMusic(&buzzer, MUSIC_RM); /* 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 */ } 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 3464669..1aadeb4 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=10.0; %机体质量 + M1=8.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,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([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 + Q=diag([160 160 2000 1500 20000 5000]);%theta d_theta x d_x phi d_phi%700 1 600 200 1000 1 + R=[140 0;0 60]; %T Tp K=lqr(A,B,Q,R); diff --git a/utils/k_calc/chassis_calc_heu.m b/utils/k_calc/chassis_calc_heu.m new file mode 100644 index 0000000..047b219 --- /dev/null +++ b/utils/k_calc/chassis_calc_heu.m @@ -0,0 +1,78 @@ +%LQR求解 +clear; + +syms theta dot_theta ddot_theta; +syms x dot_x ddot_x; +syms x_body dot_x_body ddot_x_body; +syms phi dot_phi ddot_phi; +syms T T_p; +syms R L L_M l m_w m_p M I_w I_p I_M g; +body_fusion = 1; %机体速度 +g = 9.8; %重力加速度 +R = 0.076; %轮半径 +m_w = 0.47; %轮质量 +m_p = 1.874; %摆杆质量 +M = 6.975; %机体质量 +I_w = 0.001974; %轮转动惯量 +I_M = 0.143; %机体转动惯量 +l = 0.0353; %机体质心离转轴距离 + +Q_cost=diag([160 160 200 120 2000 50]); +R_cost=diag([1, 0.25]); + +if body_fusion + ddot_x = ddot_x_body - (L+L_M)*cos(theta)*ddot_theta + (L+L_M)*sin(theta)*dot_theta^2; +end +%对机体受力分析 +N_M = M * (ddot_x + (L + L_M) * (-dot_theta^2*sin(theta) + ddot_theta*cos(theta)) - l*(-dot_phi^2*sin(phi) + ddot_phi*cos(phi))); +P_M = M*g + M*((L+L_M)*(-dot_theta^2*cos(theta) - ddot_theta*sin(theta)) + l*(-dot_phi^2*cos(phi) - ddot_phi*sin(phi))); +N = N_M + m_p*(ddot_x + L*(-dot_theta^2*sin(theta) + ddot_theta*cos(theta))); +P = P_M + m_p*g + m_p*L*(-dot_theta^2*cos(theta) - ddot_theta*sin(theta)); + +%方程求解 +equ1 = ddot_x - (T - N*R)/(I_w/R + m_w*R); +equ2 = (P*L + P_M*L_M)*sin(theta) - (N*L+N_M*L_M)*cos(theta) - T + T_p - I_p*ddot_theta; +equ3 = T_p + N_M * l * cos(phi) + P_M * l * sin(phi) - I_M * ddot_phi; +if body_fusion + [ddot_theta, ddot_x_body, ddot_phi] = solve([equ1, equ2, equ3], [ddot_theta, ddot_x_body, ddot_phi]); + Ja = jacobian([dot_theta ddot_theta dot_x_body ddot_x_body dot_phi ddot_phi], [theta, dot_theta, x_body, dot_x_body, phi, dot_phi]); + Jb = jacobian([dot_theta ddot_theta dot_x_body ddot_x_body dot_phi ddot_phi], [T, T_p]); + + A = simplify(vpa(subs(Ja, [theta, dot_theta, x_body, dot_x_body, phi, dot_phi], [0, 0, 0, 0, 0, 0]))); + B = simplify(vpa(subs(Jb, [theta, dot_theta, x_body, dot_x_body, phi, dot_phi], [0, 0, 0, 0, 0, 0]))); + +else + [ddot_theta, ddot_x, ddot_phi] = solve([equ1, equ2, equ3], [ddot_theta, ddot_x, ddot_phi]); + Ja = jacobian([dot_theta ddot_theta dot_x ddot_x dot_phi ddot_phi], [theta, dot_theta, x, dot_x, phi, dot_phi]); + Jb = jacobian([dot_theta ddot_theta dot_x ddot_x dot_phi ddot_phi], [T, T_p]); + + A = simplify(vpa(subs(Ja, [theta, dot_theta, x, dot_x, phi, dot_phi], [0, 0, 0, 0, 0, 0]))); + B = simplify(vpa(subs(Jb, [theta, dot_theta, x, dot_x, phi, dot_phi], [0, 0, 0, 0, 0, 0]))); +end + +%% LQR计算 + +leg_var = 0.096; +K=zeros(30,12); +leglen=zeros(30,1); +for i=1:30 + leg_var=leg_var+0.01; % 10mm线性化一次 + llm= 0.8424 * leg_var - 0.0272; + ll = 0.1576 * leg_var + 0.0272; + leglen(i)=leg_var; + i_p = 0.0328 * leg_var + 0.0216; + trans_A=double(subs(A,[L L_M I_p],[ll llm i_p])); + trans_B=double(subs(B,[L L_M I_p],[ll llm i_p])); + KK=lqrd(trans_A,trans_B,Q_cost,R_cost,0.001); + KK_t=KK.'; + K(i,:)=KK_t(:); +end + +%% 系数拟合 +K_cons=zeros(12,3); %排列顺序是 + +for i=1:12 + res=fit(leglen,K(:,i),'poly2'); + K_cons(i,:)=[res.p1,res.p2,res.p3]; +end +disp(K_cons) \ No newline at end of file diff --git a/utils/香港大学轮腿电控及建模开源-3/Chassis_Task.c b/utils/香港大学轮腿电控及建模开源-3/Chassis_Task.c new file mode 100644 index 0000000..aead3e0 --- /dev/null +++ b/utils/香港大学轮腿电控及建模开源-3/Chassis_Task.c @@ -0,0 +1,1347 @@ +#include "Chassis_Task.h" +#include "bsp_buzzer.h" +#include "FreeRTOS.h" +#include "task.h" +#include "detect_task.h" +#include "remote_control.h" +#include "referee.h" +#include "can.h" +#include "bsp_can.h" +#include "arm_math.h" +#include "string.h" +#include "Kalman_Mix.h" +#include "INS_Task.h" +#include "math.h" +#include "VMC.h" +#include "mpc.h" +#include "lqr.h" +#include "user_lib.h" + + +#ifndef PI +#define PI 3.14159265358979f +#endif +#ifndef FOLLOW_DEADBAND +#define FOLLOW_DEADBAND 0.05f +#endif +#ifndef min +#define min(a,b) ((a)<(b)?(a):(b)) +#endif +#ifndef square +#define square(x) ((x)*(x)) +#endif + +#define ABS(x) (((x) > 0) ? (x) : (-(x))) +#define LimitMax(input, max) \ +{ \ + if ((input) > max) \ + { \ + input = max; \ + } \ + else if ((input) < -max) \ + { \ + input = -max; \ + } \ +} +#define LimitOutput(input, min, max ) \ +{ \ + if( input < min ) \ + input = min; \ + else if( input > max ) \ + input = max; \ +} + +#define SIGN(x) ((x)>0 ? 1 : ((x)<0? -1: 0)) + + +uint8_t use_mpc = 0, abnormal_debug = 0; +chassis_control_t chassis_control; +kalman2_state Body_Speed_KF; +// ------------------ PID info ------------------ +fp32 roll_PD[2] = {100.0, 20.0}; // 200, 45 +fp32 coordinate_PD[2] = { 10.0f, 0.5f }; //15.0f,1.0f +fp32 yaw_PD_test[2] = { 20.0f, 180.0f }; +fp32 stand_PD[2] = { 200.0f, 50.0f }; +fp32 jump_stand_PD[2] = { 10000.0f, 150.0f }; +fp32 suspend_stand_PD[2] = { 100.0f, 30.0f }; +// fp32 stand_PD[2]={150.0f, 20.0f};//1N/cm +fp32 suspend_foot_speed_p = 10.0f; +// fp32 High_Offset = 0.20; +fp32 High_Offset = 0.06; +fp32 Moving_High_Offset = 0.0f; +uint8_t last_is_update; +fp32 IDEAL_PREPARING_STAND_JUMPING_ANGLE = 0.6f; + +extern fp32 LQR[4][10]; + +fp32 suspend_LQR[2][6] = { + 20.0f,5.0f, 0.0f, 0.0f, 0.0f, 0.0f, + 0, 0, 0, 0, 0, 0 +}; +fp32 x_set[10]; +fp32 debug_2 = 1,upper_bound, stepp = 0.2; +fp32 debug_flag, last_debug_flag; +fp32 HIGH_HIGH = 0.35f; + +fp32 SIT_HIGH = 0.11f; +fp32 NORMAL_HIGH = 0.18f; //16 + +fp32 lower_dec_speed = 1.0f; +uint8_t mpc_cnt = 0, mpc_period = 9; +float alpha_dx = 0.95f; +fp32 rollP, rollD, roll_angle_deadband = 0.0f, roll_gyro_deadband = 0.0f, leg_dlength_deadband = 0.0f; +fp32 rotate_move_threshold = 45.0f, rotate_speed = 12.0f, fake_sign = 1.0f, rc_sign = 1.0f, rotate_move_scale = 1.0f,normal_move_scale = 2.0f, offset_k = 0.31f, fly_speed = 2.5f; +fp32 rc_angle, rc_angle_temp, X_speed, Y_speed, delta_theta, delta_theta_temp, rotate_move_offset, normalized_speed, temp_max_spd, acc_step = 1.3f, dec_step = 0.1f; +fp32 move_scale_list[11] = { 0.0, 1.0, 1.5, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0}; +fp32 cap_move_scale_list[11] = { 0.0, 1.5, 2.0, 2.5, 2.5, 3.0, 3.0, 3.0, 3.0, 3.0, 3.0}; +fp32 rotate_speed_list[11] = {0.0, 5.0, 5.3, 5.6, 6.0, 6.0, 7.0, 8.0, 9.0, 10.0, 12.0}; +fp32 rotate_move_scale_list[11] = {0.0, 1.0, 1.2, 1.4, 1.6, 1.8, 2.0, 2.2, 2.4, 2.6, 2.8}; +int speed_sign = 0, target_speed_sign = 0; +uint8_t robot_level = 1; +ext_robot_status_t *robot; +fp32 TK_x_p = -10.0f, TK_y_p = 10.0f, TK_y_d = 3.0f, reducing_p = 120.0f; +fp32 stablize_foot_speed_threshold = 1.2f, stablize_yaw_speed_threshold = 1.5f; +uint8_t no_follow_flag, follow_angle; + +enum Chassis_Mode chassis_mode; + + +// ------------------ function definition ------------------ +void Chassis_Init( chassis_control_t *init ); +void Chassis_Data_Update( chassis_control_t *fdb ); +void Chassis_Status_Detect( chassis_control_t *detect ); +void Chassis_Mode_Set( chassis_control_t *mode_set ); +void Chassis_Mode_Change_Control_Transit( chassis_control_t *chassis_mode_change ); +void Target_Value_Set( chassis_control_t *target_value_set ); +void Chassis_Torque_Calculation( chassis_control_t *bl_ctrl ); +void Chassis_Torque_Combine( chassis_control_t *bl_ctrl ); +void Motor_CMD_Send(chassis_control_t *CMD_Send); +void Joint_Motor_to_Init_Pos(void); +void Motor_Zero_CMD_Send(void); +void HT_Motor_zero_set(void); + +int16_t set1, set2; +uint16_t launching_time = 0; + +void Chassis_Task(void const * argument) +{ + vTaskDelay(CHASSIS_TASK_INIT_TIME); + Chassis_Init(&chassis_control); + vTaskDelay(100); + while(1) + { + Chassis_Data_Update(&chassis_control); + Chassis_Status_Detect(&chassis_control); + Chassis_Mode_Set(&chassis_control); + Chassis_Mode_Change_Control_Transit(&chassis_control); + Target_Value_Set(&chassis_control); + Chassis_Torque_Calculation(&chassis_control); + Chassis_Torque_Combine(&chassis_control); + Motor_CMD_Send(&chassis_control); + vTaskDelay(1); + } +} + + +void Chassis_Init( chassis_control_t *init ) +{ + // ------------------ Set HT Zero Point ------------------ + buzzer_on(75, 60); + vTaskDelay(100); + HT_Motor_zero_set(); + buzzer_off(); + + Motor_Zero_CMD_Send(); + + kalman_second_order_init(&Body_Speed_KF); + + // ------------------ Ptr Set ------------------ + init->joint_motor_1.motor_measure = get_HT_motor_measure_point(0); + init->joint_motor_2.motor_measure = get_HT_motor_measure_point(1); + init->joint_motor_3.motor_measure = get_HT_motor_measure_point(2); + init->joint_motor_4.motor_measure = get_HT_motor_measure_point(3); + // init->foot_motor_L.motor_measure = get_BMmotor_measure_point(0); + // init->foot_motor_R.motor_measure = get_BMmotor_measure_point(1); + init->chassis_rc_ctrl = get_Gimabl_control_point(); + init->chassis_posture_info.chassis_INS_angle_point = get_INS_angle_point(); + init->chassis_posture_info.chassis_INS_gyro_point = get_gyro_data_point(); + init->chassis_posture_info.chassis_INS_accel_point = get_acc_data_point(); + + /* + first_order_filter_init(&rc_filter, 0.002f, rc_filt_numb); + first_order_filter_init(&tl_filter, 0.002f, tl_numb); + first_order_filter_init(&tl_run_filter, 0.002f, tl_numb); + OLS_Init(&OLS_S0_L,2); + OLS_Init(&OLS_S0_R,2); + */ + + // ------------------ Mode Set ------------------ + init->mode.chassis_mode = init->mode.last_chassis_mode = DISABLE_CHASSIS; + init->mode.chassis_balancing_mode = init->mode.last_chassis_balancing_mode = NO_FORCE; + init->mode.sport_mode = init->mode.last_sport_mode = NORMAL_MOVING_MODE; + init->joint_motor_1.motor_mode = init->joint_motor_1.last_motor_mode = MOTOR_NO_FORCE; + init->joint_motor_2.motor_mode = init->joint_motor_2.last_motor_mode = MOTOR_NO_FORCE; + init->joint_motor_3.motor_mode = init->joint_motor_3.last_motor_mode = MOTOR_NO_FORCE; + init->joint_motor_4.motor_mode = init->joint_motor_4.last_motor_mode = MOTOR_NO_FORCE; + init->foot_motor_L.motor_mode = init->foot_motor_L.last_motor_mode = MOTOR_NO_FORCE; + init->foot_motor_R.motor_mode = init->foot_motor_R.last_motor_mode = MOTOR_NO_FORCE; + + // ------------------ Initialize HT Motor ------------------ + init->joint_motor_1.position_offset = init->joint_motor_1.motor_measure->ecd; + init->joint_motor_2.position_offset = init->joint_motor_2.motor_measure->ecd; + init->joint_motor_3.position_offset = init->joint_motor_3.motor_measure->ecd; + init->joint_motor_4.position_offset = init->joint_motor_4.motor_measure->ecd; + init->joint_motor_1.position = (init->joint_motor_1.motor_measure->ecd -init->joint_motor_1.position_offset) + LEG_OFFSET; + init->joint_motor_2.position = (init->joint_motor_2.motor_measure->ecd -init->joint_motor_2.position_offset) - LEG_OFFSET; + init->joint_motor_3.position = (init->joint_motor_3.motor_measure->ecd -init->joint_motor_3.position_offset) + LEG_OFFSET; + init->joint_motor_4.position = (init->joint_motor_4.motor_measure->ecd -init->joint_motor_4.position_offset) - LEG_OFFSET; + + init->flag_info.init_flag = 1; + Chassis_Data_Update(init); + init->flag_info.init_flag = 0; + + init->foot_motor_L.distance_offset = init->foot_motor_L.distance; + init->foot_motor_R.distance_offset = init->foot_motor_R.distance; + + Power_Init(init); + + // ------------------ MPC init ------------------ + MPC_Init(); +} + +void Chassis_Data_Update( chassis_control_t *fdb ) +{ + robot = get_robot_status_point(); + robot_level = robot->robot_level; + // robot_level = 1; + // -------------------- Update foot measure -------------------- + fdb->foot_motor_L.motor_measure.ecd = (uint16_t)motor_BM[0].ecd; + fdb->foot_motor_L.motor_measure.last_ecd = (uint16_t)motor_BM[0].last_ecd; + fdb->foot_motor_L.motor_measure.speed_rpm = (int16_t)motor_BM[0].speed_rpm; + fdb->foot_motor_L.torque_get = motor_BM[0].given_current*55.0f/32767.0f; + + fdb->foot_motor_R.motor_measure.ecd = (uint16_t)motor_BM[1].ecd; + fdb->foot_motor_R.motor_measure.last_ecd = (uint16_t)motor_BM[1].last_ecd; + fdb->foot_motor_R.motor_measure.speed_rpm = (int16_t)motor_BM[1].speed_rpm; + fdb->foot_motor_R.torque_get = motor_BM[1].given_current*55.0f/32767.0f; + + // ------------------ Update mode info ------------------ + fdb->mode.last_chassis_mode = fdb->mode.chassis_mode; + fdb->mode.last_chassis_balancing_mode = fdb->mode.chassis_balancing_mode; + fdb->mode.last_sport_mode = fdb->mode.sport_mode; + fdb->mode.last_jumping_mode = fdb->mode.jumping_mode; + fdb->mode.last_jumping_stage = fdb->mode.jumping_stage; + fdb->mode.last_chassis_high_mode = fdb->mode.chassis_high_mode; + fdb->flag_info.last_static_flag = fdb->flag_info.static_flag; + fdb->flag_info.last_moving_flag = fdb->flag_info.moving_flag; + last_debug_flag = debug_flag; + fdb->flag_info.last_overpower_warning_flag = fdb->flag_info.overpower_warning_flag; + fdb->flag_info.last_stablize_high_flag = fdb->flag_info.stablize_high_flag; + + fdb->flag_info.last_suspend_flag_L = fdb->flag_info.suspend_flag_L; + fdb->flag_info.last_suspend_flag_R = fdb->flag_info.suspend_flag_R; + + fdb->joint_motor_1.last_motor_mode = fdb->joint_motor_1.motor_mode; + fdb->joint_motor_2.last_motor_mode = fdb->joint_motor_2.motor_mode; + fdb->joint_motor_3.last_motor_mode = fdb->joint_motor_3.motor_mode; + fdb->joint_motor_4.last_motor_mode = fdb->joint_motor_4.motor_mode; + fdb->foot_motor_L.last_motor_mode = fdb->foot_motor_L.motor_mode; + fdb->foot_motor_R.last_motor_mode = fdb->foot_motor_R.motor_mode; + + // ------------------ Update IMU info ------------------ + fdb->chassis_posture_info.pitch_angle = *(fdb->chassis_posture_info.chassis_INS_angle_point + INS_PITCH_ADDRESS_OFFSET); + fdb->chassis_posture_info.roll_angle = *(fdb->chassis_posture_info.chassis_INS_angle_point + INS_ROLL_ADDRESS_OFFSET); + fdb->chassis_posture_info.yaw_angle = *(fdb->chassis_posture_info.chassis_INS_angle_point + INS_YAW_ADDRESS_OFFSET) + yaw_count * PI2; + fdb->chassis_posture_info.pitch_gyro = *(fdb->chassis_posture_info.chassis_INS_gyro_point + INS_PITCH_ADDRESS_OFFSET); + fdb->chassis_posture_info.roll_gyro = *(fdb->chassis_posture_info.chassis_INS_gyro_point + INS_ROLL_ADDRESS_OFFSET); + fdb->chassis_posture_info.yaw_gyro = *(fdb->chassis_posture_info.chassis_INS_gyro_point + INS_YAW_ADDRESS_OFFSET); + + // ------------------ Update HT Motor info ------------------ + fdb->joint_motor_1.position = (fdb->joint_motor_1.motor_measure->ecd - fdb->joint_motor_1.position_offset) + LEG_OFFSET; + fdb->joint_motor_2.position = (fdb->joint_motor_2.motor_measure->ecd - fdb->joint_motor_2.position_offset) - LEG_OFFSET; + fdb->joint_motor_3.position = (fdb->joint_motor_3.motor_measure->ecd - fdb->joint_motor_3.position_offset) + LEG_OFFSET; + fdb->joint_motor_4.position = (fdb->joint_motor_4.motor_measure->ecd - fdb->joint_motor_4.position_offset) - LEG_OFFSET; + + fdb->joint_motor_1.velocity = fdb->joint_motor_1.motor_measure->speed_rpm; + fdb->joint_motor_2.velocity = fdb->joint_motor_2.motor_measure->speed_rpm; + fdb->joint_motor_3.velocity = fdb->joint_motor_3.motor_measure->speed_rpm; + fdb->joint_motor_4.velocity = fdb->joint_motor_4.motor_measure->speed_rpm; + + fdb->joint_motor_1.torque_get = 0.95f * fdb->joint_motor_1.torque_get + 0.05f * fdb->joint_motor_1.motor_measure->real_torque; + fdb->joint_motor_2.torque_get = 0.95f * fdb->joint_motor_2.torque_get + 0.05f * fdb->joint_motor_2.motor_measure->real_torque; + fdb->joint_motor_3.torque_get = 0.95f * fdb->joint_motor_3.torque_get + 0.05f * fdb->joint_motor_3.motor_measure->real_torque; + fdb->joint_motor_4.torque_get = 0.95f * fdb->joint_motor_4.torque_get + 0.05f * fdb->joint_motor_4.motor_measure->real_torque; + + // ------------------ Update LK Motor info ------------------ + fdb->foot_motor_L.last_position = fdb->foot_motor_L.position; + fdb->foot_motor_R.last_position = fdb->foot_motor_R.position; + fdb->foot_motor_L.position = fdb->foot_motor_L.motor_measure.ecd * MOTOR_ECD_TO_RAD;// Angle (rad) + fdb->foot_motor_R.position = fdb->foot_motor_R.motor_measure.ecd * MOTOR_ECD_TO_RAD; + + if( fdb->flag_info.init_flag != 1 ) // Init_flag = 0: after Init fdb 如果不是在init里面调用就�?�圈 + { + if( (fdb->foot_motor_L.last_position - fdb->foot_motor_L.position ) > HALF_POSITION_RANGE ) + fdb->foot_motor_L.turns++; + else if( (fdb->foot_motor_L.last_position - fdb->foot_motor_L.position ) < -HALF_POSITION_RANGE ) + fdb->foot_motor_L.turns--; + if( ( fdb->foot_motor_R.last_position - fdb->foot_motor_R.position ) > HALF_POSITION_RANGE ) + fdb->foot_motor_R.turns--; + else if( ( fdb->foot_motor_R.last_position - fdb->foot_motor_R.position ) < -HALF_POSITION_RANGE ) + fdb->foot_motor_R.turns++; + } + fdb->foot_motor_L.distance = ( fdb->foot_motor_L.position/PI2 + fdb->foot_motor_L.turns ) * WHEEL_PERIMETER - fdb->foot_motor_L.distance_offset; + fdb->foot_motor_R.distance = ( -fdb->foot_motor_R.position/PI2 + fdb->foot_motor_R.turns ) * WHEEL_PERIMETER - fdb->foot_motor_R.distance_offset; + fdb->chassis_posture_info.foot_distance = ( fdb->foot_motor_L.distance + fdb->foot_motor_R.distance ) /2.0f; + + fdb->foot_motor_L.speed = fdb->foot_motor_L.motor_measure.speed_rpm * PI2 * WHEEL_RADIUS / 10.0f / 60.0f; // rpm -> m/s + fdb->foot_motor_R.speed = -fdb->foot_motor_R.motor_measure.speed_rpm * PI2 * WHEEL_RADIUS / 10.0f / 60.0f; + fdb->chassis_posture_info.foot_speed = ( fdb->foot_motor_L.speed + fdb->foot_motor_R.speed ) / 2.0f; + + // fdb->foot_motor_L.torque_get = fdb->foot_motor_L.motor_measure.given_current; + // fdb->foot_motor_R.torque_get = fdb->foot_motor_R.motor_measure.given_current; + + // ------------------ Kalman Filter ------------------ + + kalman_second_order_update(&Body_Speed_KF, fdb->chassis_posture_info.foot_distance, fdb->chassis_posture_info.foot_speed, temp_a); + fdb->chassis_posture_info.foot_distance_K = Body_Speed_KF.x[0]; + fdb->chassis_posture_info.foot_speed_K = Body_Speed_KF.x[1]; + + // ------------------ Five_Bars_to_Pendulum ------------------ + Forward_kinematic_solution(fdb,-fdb->joint_motor_2.position,-fdb->joint_motor_2.velocity, + -fdb->joint_motor_1.position,-fdb->joint_motor_1.velocity,1); + Forward_kinematic_solution(fdb,fdb->joint_motor_3.position,fdb->joint_motor_3.velocity, + fdb->joint_motor_4.position, fdb->joint_motor_4.velocity,0); + + // ------------------ Huanzhi information update ------------------ + fdb->chassis_posture_info.leg_angle_L -= PI_2; + fdb->chassis_posture_info.leg_angle_L -= fdb->chassis_posture_info.pitch_angle; + // fdb->chassis_posture_info.leg_gyro_L -= fdb->chassis_posture_info.pitch_gyro; + fp32 temp_v_L = ( fdb->chassis_posture_info.leg_angle_L - fdb->chassis_posture_info.last_leg_angle_L ) / TASK_RUN_TIME; + fdb->chassis_posture_info.leg_gyro_L = alpha_dx * temp_v_L + (1-alpha_dx) * fdb->chassis_posture_info.leg_gyro_L; + fdb->chassis_posture_info .last_leg_angle_L = fdb->chassis_posture_info .leg_angle_L; + + fdb->chassis_posture_info.leg_angle_R -= PI_2; + fdb->chassis_posture_info.leg_angle_R -= fdb->chassis_posture_info.pitch_angle; + // fdb->chassis_posture_info.leg_gyro_R -= fdb->chassis_posture_info.pitch_gyro; + fp32 temp_v_R = ( fdb->chassis_posture_info.leg_angle_R - fdb->chassis_posture_info.last_leg_angle_R ) / TASK_RUN_TIME; + fdb->chassis_posture_info.leg_gyro_R = alpha_dx * temp_v_R + (1-alpha_dx) * fdb->chassis_posture_info.leg_gyro_R; + fdb->chassis_posture_info.last_leg_angle_R = fdb->chassis_posture_info .leg_angle_R; + + temp_v_L = ( fdb->chassis_posture_info.leg_length_L - fdb->chassis_posture_info.last_leg_length_L ) / TASK_RUN_TIME; + fdb->chassis_posture_info.leg_dlength_L = alpha_dx * temp_v_L + (1-alpha_dx) * fdb->chassis_posture_info.leg_dlength_L; + fdb->chassis_posture_info.last_leg_length_L = fdb->chassis_posture_info.leg_length_L; + + temp_v_R = ( fdb->chassis_posture_info.leg_length_R - fdb->chassis_posture_info.last_leg_length_R ) / TASK_RUN_TIME; + fdb->chassis_posture_info.leg_dlength_R = alpha_dx * temp_v_R + (1-alpha_dx) * fdb->chassis_posture_info.leg_dlength_R; + fdb->chassis_posture_info.last_leg_length_R = fdb->chassis_posture_info.leg_length_R; + + // ---------- Rotate and Move Info update ------------ + rotate_move_offset = offset_k * rotate_speed_list[robot_level]; + rc_sign = 1.0f; + X_speed = fdb->chassis_rc_ctrl->X_speed; + Y_speed = fdb->chassis_rc_ctrl->Y_speed; + // Original rc angle + if (X_speed == 0){ + if (Y_speed > 0) + rc_angle_temp = PI/2; + else if (Y_speed < 0) + rc_angle_temp = -PI/2; + else + rc_angle_temp = 0; + } + else{ + Y_speed = fdb->chassis_rc_ctrl->Y_speed; + rc_angle_temp = atan_tl(Y_speed/X_speed); + } + // Normalized speed + if (abs(X_speed) < 0.1 ||abs(Y_speed) <0.1) + temp_max_spd = 1.0f; + else if (X_speedPI/2||rc_angle_temp<-PI/2){ + rc_sign *= -1.0f; + } + while (rc_angle_temp>PI/2) { + rc_angle_temp -= PI; + } + while (rc_angle_temp<-PI/2){ + rc_angle_temp += PI; + } + // if (X_speed < 0) rc_sign = -1.0f; + rc_angle = rc_angle_temp * 180.0f / PI; + if (fdb->chassis_rc_ctrl->fake_flag == 0) fake_sign = -1.0f; + else fake_sign = 1.0f; +} + +void Chassis_Status_Detect( chassis_control_t *detect ) +{ + // ------------------ Off Ground Detect ------------------ + + + if( detect->mode.chassis_balancing_mode == BALANCING_READY && detect->mode.sport_mode!=TK_MODE) + { + if( ABS(detect->chassis_posture_info.pitch_angle) >= DANGER_PITCH_ANGLE ) + detect->flag_info.abnormal_flag = 1; + else if( ABS(detect->chassis_posture_info.foot_speed_K) < MOVE_LOWER_BOUND && + ABS(detect->chassis_posture_info.pitch_angle) < 0.1f && + detect->flag_info.abnormal_flag ) + detect->flag_info.abnormal_flag = 0; + } + if (abnormal_debug){ + detect->flag_info.abnormal_flag = 0; + } + + Supportive_Force_Cal(detect, detect->joint_motor_1.position, detect->joint_motor_2.position, 1.0f ); + Supportive_Force_Cal(detect, detect->joint_motor_3.position, detect->joint_motor_4.position, 0.0f ); + + if( detect->mode.jumping_stage == CONSTACTING_LEGS ) + detect->flag_info.suspend_flag_L = detect->flag_info.suspend_flag_R = ON_GROUND; + else + { + if( detect->mode.sport_mode == JUMPING_MODE ) + { + if( detect->chassis_posture_info.supportive_force_R <= LOWER_SUPPORT_FORCE_FOR_JUMP && + detect->chassis_posture_info.leg_length_R > 0.13f ) + detect->flag_info.suspend_flag_R = OFF_GROUND; + else + detect->flag_info.suspend_flag_L = ON_GROUND; + if( detect->chassis_posture_info.supportive_force_L <= LOWER_SUPPORT_FORCE_FOR_JUMP && + detect->chassis_posture_info.leg_length_L > 0.13f ) + detect->flag_info.suspend_flag_L = OFF_GROUND; + else + detect->flag_info.suspend_flag_R = ON_GROUND; + } + else + { + if( detect->chassis_posture_info.supportive_force_R <= LOWER_SUPPORT_FORCE && + detect->chassis_posture_info.leg_length_R > 0.13f ) + detect->flag_info.suspend_flag_R = OFF_GROUND; + else + detect->flag_info.suspend_flag_L = ON_GROUND; + if( detect->chassis_posture_info.supportive_force_L <= LOWER_SUPPORT_FORCE && + detect->chassis_posture_info.leg_length_L > 0.13f ) + detect->flag_info.suspend_flag_L = OFF_GROUND; + else + detect->flag_info.suspend_flag_R = ON_GROUND; + } + } + + + if( detect->flag_info.abnormal_flag == 1 && + ( detect->flag_info.last_suspend_flag_L == ON_GROUND || detect->flag_info.last_suspend_flag_R == ON_GROUND ) && + ( detect->flag_info.suspend_flag_L == OFF_GROUND || detect->flag_info.suspend_flag_R == OFF_GROUND ) ) + detect->flag_info.Ignore_Off_Ground = 1; + else if( detect->flag_info.abnormal_flag != 1 ) + detect->flag_info.Ignore_Off_Ground = 0; + if( detect->flag_info.Ignore_Off_Ground ) + { + detect->flag_info.suspend_flag_R = ON_GROUND; + detect->flag_info.suspend_flag_L = ON_GROUND; + } + // detect->flag_info.suspend_flag_L = detect->flag_info.suspend_flag_R = ON_GROUND; + //Moving_High_Offset + + if( ABS(detect->chassis_posture_info.foot_speed_K) > stablize_foot_speed_threshold && + ABS(detect->chassis_posture_info.yaw_gyro ) > stablize_yaw_speed_threshold ) + detect->flag_info.stablize_high_flag = 1; + +} + +void Chassis_Mode_Set( chassis_control_t *mode_set ) +{ + // ----------------- Chassis Mode Update ----------------- + if( mode_set->chassis_rc_ctrl->mode_R == 0 || toe_is_error(GIMBAL_TOE)|| toe_is_error(BM1_TOE)||toe_is_error(BM2_TOE)) + mode_set->mode.chassis_mode = DISABLE_CHASSIS; + else + mode_set->mode.chassis_mode = ENABLE_CHASSIS; + + if( mode_set->mode.chassis_mode == ENABLE_CHASSIS ) + { + mode_set->joint_motor_1.motor_mode = MOTOR_FORCE; + mode_set->joint_motor_2.motor_mode = MOTOR_FORCE; + mode_set->joint_motor_3.motor_mode = MOTOR_FORCE; + mode_set->joint_motor_4.motor_mode = MOTOR_FORCE; + mode_set->foot_motor_L.motor_mode = MOTOR_FORCE; + mode_set->foot_motor_R.motor_mode = MOTOR_FORCE; + } + else + { + if( mode_set->mode.chassis_balancing_mode == JOINT_REDUCING ) + { + mode_set->joint_motor_1.motor_mode = MOTOR_FORCE; + mode_set->joint_motor_2.motor_mode = MOTOR_FORCE; + mode_set->joint_motor_3.motor_mode = MOTOR_FORCE; + mode_set->joint_motor_4.motor_mode = MOTOR_FORCE; + mode_set->foot_motor_L.motor_mode = MOTOR_NO_FORCE; + mode_set->foot_motor_R.motor_mode = MOTOR_NO_FORCE; + } + else + { + mode_set->joint_motor_1.motor_mode = MOTOR_NO_FORCE; + mode_set->joint_motor_2.motor_mode = MOTOR_NO_FORCE; + mode_set->joint_motor_3.motor_mode = MOTOR_NO_FORCE; + mode_set->joint_motor_4.motor_mode = MOTOR_NO_FORCE; + mode_set->foot_motor_L.motor_mode = MOTOR_NO_FORCE; + mode_set->foot_motor_R.motor_mode = MOTOR_NO_FORCE; + } + + } + + // ----------------- Sport Mode Update ----------------- + if( mode_set->mode.chassis_balancing_mode == BALANCING_READY ) + { + if( mode_set->chassis_rc_ctrl->tk_flag ) + mode_set->mode.sport_mode = TK_MODE; + else if( mode_set->flag_info.abnormal_flag ) + mode_set->mode.sport_mode = ABNORMAL_MOVING_MODE; + else if( mode_set->mode.sport_mode == JUMPING_MODE && mode_set->mode.jumping_stage != FINISHED ) + mode_set->mode.sport_mode = JUMPING_MODE; + else if( mode_set->chassis_rc_ctrl->jump_flag ) + mode_set->mode.sport_mode = JUMPING_MODE; + else if( mode_set->chassis_rc_ctrl->cap_flag ) + mode_set->mode.sport_mode = CAP_MODE; + else if( mode_set->chassis_rc_ctrl->fly_flag ) + mode_set->mode.sport_mode = FLY_MODE; + else + mode_set->mode.sport_mode = NORMAL_MOVING_MODE; + } + else + mode_set->mode.sport_mode = NONE; + + // ----------------- Rotation Flag -------------------- + static uint8_t last_rotation_flag = 0; + last_rotation_flag = mode_set->flag_info.rotation_flag; + mode_set->flag_info.rotation_flag = mode_set->chassis_rc_ctrl->mode_R == 4; //Rotate + if (!last_rotation_flag && mode_set->flag_info.rotation_flag){ + for (int i = 0; i < 11; ++i) + rotate_speed_list[i] = - rotate_speed_list[i]; + } + + // ----------------- No Follow Flag ---------------------- + if (toe_is_error(GIMBAL_TOE)) no_follow_flag = 1; + else no_follow_flag = mode_set->chassis_rc_ctrl->mode_R == 3; //No Follow + + // ---------------- Moving Flag ---------------------------- + if( mode_set->chassis_rc_ctrl->X_speed != 0.0f ) + { + mode_set->flag_info.controlling_flag = 1; + mode_set->flag_info.set_pos_after_moving = 1; + } + else + mode_set->flag_info.controlling_flag = 0; + + if( ABS(mode_set->chassis_posture_info.foot_speed_K) <= 0.05f && + !mode_set->flag_info.controlling_flag && + mode_set->flag_info.set_pos_after_moving ) // maybe bug + { + mode_set->flag_info.moving_flag = 0; + mode_set->flag_info.set_pos_after_moving = 0; + } + else + mode_set->flag_info.moving_flag = 1; + + +} + +uint8_t reduce_flag = 0; +fp32 reduce_high, high_offset = 0.2f; +fp32 debug_1 = 0.995; +void Chassis_Mode_Change_Control_Transit( chassis_control_t *chassis_mode_change ) +{ + if( chassis_mode_change->mode.chassis_mode == ENABLE_CHASSIS && chassis_mode_change->mode.last_chassis_mode == DISABLE_CHASSIS ) + chassis_mode_change->mode.chassis_balancing_mode = FOOT_LAUNCHING; + + if( chassis_mode_change->mode.chassis_balancing_mode == FOOT_LAUNCHING && + ABS(chassis_mode_change->chassis_posture_info.pitch_angle) < EXIT_PITCH_ANGLE ) + chassis_mode_change->mode.chassis_balancing_mode = JOINT_LAUNCHING; + else if( chassis_mode_change->mode.chassis_balancing_mode == JOINT_LAUNCHING && ( + (NORMAL_HIGH - chassis_mode_change->chassis_posture_info.leg_length_L) < 0.03f || + (NORMAL_HIGH - chassis_mode_change->chassis_posture_info.leg_length_R) < 0.03f) ) + chassis_mode_change->mode.chassis_balancing_mode = BALANCING_READY; + else if( chassis_mode_change->mode.chassis_balancing_mode == BALANCING_READY && + chassis_mode_change->mode.chassis_mode == DISABLE_CHASSIS ){ + if (!reduce_flag) { + reduce_high = chassis_mode_change->chassis_posture_info.ideal_high + high_offset; + reduce_flag = 1; + } + chassis_mode_change->mode.chassis_balancing_mode = JOINT_REDUCING; + } + else if( chassis_mode_change->mode.chassis_balancing_mode == JOINT_REDUCING && ( + ABS( chassis_mode_change->chassis_posture_info.leg_length_L - SIT_HIGH ) < 0.001f || + ABS( chassis_mode_change->chassis_posture_info.leg_length_R - SIT_HIGH ) < 0.001f ) ){ + chassis_mode_change->mode.chassis_balancing_mode = NO_FORCE; + reduce_flag = 0; + } + + if( chassis_mode_change->mode.sport_mode == JUMPING_MODE && chassis_mode_change->mode.last_sport_mode != JUMPING_MODE ) + { + chassis_mode_change->mode.jumping_mode = STANDING_JUMP; + } + else if( chassis_mode_change->mode.sport_mode != JUMPING_MODE ) + chassis_mode_change->mode.jumping_mode = NOT_DEFINE; + + + if( chassis_mode_change->mode.jumping_mode == MOVING_JUMP ) + { + if( chassis_mode_change->mode.jumping_stage == READY_TO_JUMP ) + chassis_mode_change->mode.jumping_stage = CONSTACTING_LEGS; + else if( chassis_mode_change->mode.jumping_stage == CONSTACTING_LEGS && + chassis_mode_change->chassis_posture_info.leg_length_L <= 0.13f ) + chassis_mode_change->mode.jumping_stage = EXTENDING_LEGS; + else if( chassis_mode_change->mode.jumping_stage == EXTENDING_LEGS && + chassis_mode_change->chassis_posture_info.leg_length_L >= 0.30f ) + chassis_mode_change->mode.jumping_stage = CONSTACTING_LEGS_2; + else if( chassis_mode_change->mode.jumping_stage == CONSTACTING_LEGS_2 && + chassis_mode_change->chassis_posture_info.leg_length_L <= 0.13f ) + chassis_mode_change->mode.jumping_stage = PREPARING_LANDING; + else if( chassis_mode_change->mode.jumping_stage == PREPARING_LANDING && + chassis_mode_change->flag_info.suspend_flag_R == ON_GROUND && + chassis_mode_change->flag_info.suspend_flag_L == ON_GROUND ) + chassis_mode_change->mode.jumping_stage = FINISHED; + else if( chassis_mode_change->mode.jumping_stage == FINISHED ) + chassis_mode_change->mode.jumping_stage = READY_TO_JUMP; + } + else if( chassis_mode_change->mode.jumping_mode == STANDING_JUMP ) + { + if( chassis_mode_change->mode.jumping_stage == READY_TO_JUMP ) + chassis_mode_change->mode.jumping_stage = PREPARING_STAND_JUMPING; + else if(chassis_mode_change->mode.jumping_stage == PREPARING_STAND_JUMPING && + ABS(chassis_mode_change->chassis_posture_info.leg_angle_L + IDEAL_PREPARING_STAND_JUMPING_ANGLE ) < stepp ) + chassis_mode_change->mode.jumping_stage = EXTENDING_LEGS; + else if( chassis_mode_change->mode.jumping_stage == EXTENDING_LEGS && + chassis_mode_change->chassis_posture_info.leg_length_L >= 0.30f ) + chassis_mode_change->mode.jumping_stage = CONSTACTING_LEGS_2; + else if( chassis_mode_change->mode.jumping_stage == CONSTACTING_LEGS_2 && + chassis_mode_change->chassis_posture_info.leg_length_L <= 0.13f ) + chassis_mode_change->mode.jumping_stage = PREPARING_LANDING; + else if( chassis_mode_change->mode.jumping_stage == PREPARING_LANDING && + chassis_mode_change->flag_info.suspend_flag_R == ON_GROUND && + chassis_mode_change->flag_info.suspend_flag_L == ON_GROUND ) + chassis_mode_change->mode.jumping_stage = FINISHED; + else if( chassis_mode_change->mode.jumping_stage == FINISHED ) + chassis_mode_change->mode.jumping_stage = READY_TO_JUMP; + } + else + chassis_mode_change->mode.jumping_stage = FINISHED; + + + if( chassis_mode_change->flag_info.stablize_high_flag == 1 && + Moving_High_Offset >= 0.2 && + chassis_mode_change->chassis_posture_info.yaw_gyro <= ( stablize_yaw_speed_threshold - 0.5f ) ) + chassis_mode_change->flag_info.stablize_high_flag = 0; +} + +fp32 HIGH_SWITCH = 36.0f; + +void Target_Value_Set( chassis_control_t *target_value_set ) +{ + // ------------------------------------ + if( target_value_set->flag_info.stablize_high_flag == 1 ) + if( Moving_High_Offset < 0.2 ) + Moving_High_Offset += 0.001f; + + if( target_value_set->flag_info.stablize_high_flag == 0 ) + Moving_High_Offset = 0.0f; + + // --------- X Speed Set ---------- + if( target_value_set->mode.sport_mode != NONE && + target_value_set->flag_info.suspend_flag_L == ON_GROUND && + target_value_set->flag_info.suspend_flag_R == ON_GROUND ) + { + if (!target_value_set->flag_info.rotation_flag) {// Normal move + if(toe_is_error(REFEREE_TOE)){ + target_value_set->chassis_posture_info.foot_speed_set = fake_sign * target_value_set->chassis_rc_ctrl->X_speed * normal_move_scale ; + + } else { + // target_speed_sign = (fake_sign * target_value_set->chassis_rc_ctrl->X_speed * move_scale_list[robot_level]) > 0 ? 1: -1; + target_speed_sign = SIGN(fake_sign * target_value_set->chassis_rc_ctrl->X_speed * move_scale_list[robot_level]); + target_value_set->chassis_posture_info.foot_speed_set = target_value_set->chassis_posture_info.foot_speed_K + target_speed_sign * acc_step; + + if (target_value_set->mode.sport_mode == FLY_MODE) + target_value_set->chassis_posture_info.foot_speed_set = target_speed_sign * min(ABS(target_value_set->chassis_rc_ctrl->X_speed * fly_speed),ABS(target_value_set->chassis_posture_info.foot_speed_set)); + else if (target_value_set->mode.sport_mode == CAP_MODE) + target_value_set->chassis_posture_info.foot_speed_set = target_speed_sign * min(ABS(target_value_set->chassis_rc_ctrl->X_speed * cap_move_scale_list[robot_level]),ABS(target_value_set->chassis_posture_info.foot_speed_set)); + else + target_value_set->chassis_posture_info.foot_speed_set = target_speed_sign * min(ABS(target_value_set->chassis_rc_ctrl->X_speed * move_scale_list[robot_level]),ABS(target_value_set->chassis_posture_info.foot_speed_set)); + + // if( target_value_set->chassis_posture_info.leg_length_L > 0.22f || target_value_set->chassis_posture_info.leg_length_R > 0.22f ) + // { + // target_value_set->chassis_posture_info.foot_speed_set /= 2.0f; + // debug_2++; + // } + } + + } + else // Rotate and move + { + delta_theta_temp = target_value_set->chassis_rc_ctrl->W_angle - rc_angle; + if (delta_theta_temp>PI/2) delta_theta_temp -= PI; + else if (delta_theta_temp<-PI/2) delta_theta_temp += PI; + delta_theta = abs(delta_theta_temp); + if (delta_thetachassis_posture_info.foot_speed_set + = ((rotate_move_threshold-delta_theta)/rotate_move_threshold) + * rc_sign * fake_sign * normalized_speed * rotate_move_scale_list[robot_level]; + else target_value_set->chassis_posture_info.foot_speed_set = 0; + } + + } + else + target_value_set->chassis_posture_info.foot_speed_set = 0; + + // --------- Distance Set --------- + if( target_value_set->mode.chassis_balancing_mode == NO_FORCE || target_value_set->mode.sport_mode == TK_MODE) + target_value_set->chassis_posture_info.foot_distance_set = target_value_set->chassis_posture_info.foot_distance_K; + else if( target_value_set->mode.sport_mode == ABNORMAL_MOVING_MODE ) + target_value_set->chassis_posture_info.foot_distance_set = target_value_set->chassis_posture_info.foot_distance_set; + else if( target_value_set->flag_info.suspend_flag_R == OFF_GROUND || + target_value_set->flag_info.suspend_flag_L == OFF_GROUND ) + target_value_set->chassis_posture_info.foot_distance_set = target_value_set->chassis_posture_info.foot_distance_K; + else if( target_value_set->flag_info.controlling_flag ) + target_value_set->chassis_posture_info.foot_distance_set = target_value_set->chassis_posture_info.foot_distance_K; + else if( !target_value_set->flag_info.moving_flag && + target_value_set->mode.chassis_balancing_mode == BALANCING_READY && + target_value_set->flag_info.last_moving_flag ) + target_value_set->chassis_posture_info.foot_distance_set = target_value_set->chassis_posture_info.foot_distance_K; + + + + // --------- Y Speed & Angle Set --------- + if( target_value_set->mode.sport_mode != NONE && + target_value_set->flag_info.suspend_flag_L == ON_GROUND && + target_value_set->flag_info.suspend_flag_R == ON_GROUND ) + { + if( target_value_set->flag_info.rotation_flag == 1 ) + { + target_value_set->chassis_posture_info.yaw_angle_sett = target_value_set->chassis_posture_info.yaw_angle; + target_value_set->chassis_posture_info.yaw_gyro_set = rotate_speed_list[robot_level]; + } + else + { + if (target_value_set->chassis_rc_ctrl->W_angle<-FOLLOW_DEADBAND) + target_value_set->chassis_posture_info.yaw_angle_sett = target_value_set->chassis_posture_info.yaw_angle + (target_value_set->chassis_rc_ctrl->W_angle+FOLLOW_DEADBAND) / 180.0f * PI; + else if (target_value_set->chassis_rc_ctrl->W_angle>FOLLOW_DEADBAND) + target_value_set->chassis_posture_info.yaw_angle_sett = target_value_set->chassis_posture_info.yaw_angle + (target_value_set->chassis_rc_ctrl->W_angle-FOLLOW_DEADBAND) / 180.0f * PI; + else + target_value_set->chassis_posture_info.yaw_angle_sett = target_value_set->chassis_posture_info.yaw_angle; + target_value_set->chassis_posture_info.yaw_gyro_set = 0.0f; + } + } + else + { + target_value_set->chassis_posture_info.yaw_angle_sett = target_value_set->chassis_posture_info.yaw_angle; + target_value_set->chassis_posture_info.yaw_gyro_set = target_value_set->chassis_posture_info.yaw_gyro; + } + + + + // --------- Side Angle Set --------- + // if( target_value_set->mode.sport_mode != NONE && + // target_value_set->mode.sport_mode != ABNORMAL_MOVING_MODE && + // target_value_set->flag_info.suspend_flag_R == ON_GROUND && + // target_value_set->flag_info.suspend_flag_L == ON_GROUND ) + // { + // if( target_value_set->mode.sport_mode == SIDE_MODE ) + // target_value_set->chassis_posture_info.roll_angle_set = (fp32)(5.0 / 180.0 * PI) * 50.0f; + // // else if( target_value_set->chassis_rc_ctrl->side_flag == -1 ) + // // target_value_set->chassis_posture_info.roll_angle_set = -(fp32)(5.0 / 180.0 * PI) * 50.0f; + // else + // target_value_set->chassis_posture_info.roll_angle_set = 0.0f; + // } + // else + target_value_set->chassis_posture_info.roll_angle_set = 0.0f; + + // ---------------- Leg Angle Set ---------- + if( target_value_set->mode.jumping_stage == PREPARING_STAND_JUMPING ) + { + target_value_set->chassis_posture_info.leg_angle_L_set = -IDEAL_PREPARING_STAND_JUMPING_ANGLE; + target_value_set->chassis_posture_info.leg_angle_R_set = -IDEAL_PREPARING_STAND_JUMPING_ANGLE; + } + else + { + target_value_set->chassis_posture_info.leg_angle_L_set = 0.0f; + target_value_set->chassis_posture_info.leg_angle_R_set = 0.0f; + } + + + // ----------------- Chassis High Mode Update ----------------- + if( target_value_set->mode.sport_mode == ABNORMAL_MOVING_MODE || target_value_set->mode.sport_mode == TK_MODE) + target_value_set->mode.chassis_high_mode = SIT_MODE; + else if( target_value_set->mode.chassis_balancing_mode == FOOT_LAUNCHING ) + target_value_set->mode.chassis_high_mode = SIT_MODE; + else if( target_value_set->mode.chassis_balancing_mode == JOINT_LAUNCHING ) + target_value_set->mode.chassis_high_mode = NORMAL_MODE; + else if( target_value_set->mode.chassis_balancing_mode == JOINT_REDUCING ) + target_value_set->mode.chassis_high_mode = CHANGING_HIGH; + else if( target_value_set->chassis_rc_ctrl->sit_flag ) + target_value_set->mode.chassis_high_mode = SIT_MODE; + else if( target_value_set->chassis_rc_ctrl->high_flag ) + target_value_set->mode.chassis_high_mode = HIGH_MODE; + else if( target_value_set->mode.jumping_stage == CONSTACTING_LEGS ) + target_value_set->mode.chassis_high_mode = SIT_MODE; + else if( target_value_set->mode.jumping_stage == EXTENDING_LEGS ) + target_value_set->mode.chassis_high_mode = HIGH_MODE; + else if( target_value_set->mode.jumping_stage == CONSTACTING_LEGS_2 ) + target_value_set->mode.chassis_high_mode = SIT_MODE; + else if( target_value_set->mode.jumping_stage == PREPARING_LANDING ) + target_value_set->mode.chassis_high_mode = NORMAL_MODE; + else + target_value_set->mode.chassis_high_mode = NORMAL_MODE; + + // --------- Leg Length Set --------- + if( target_value_set->mode.chassis_high_mode == SIT_MODE ) + target_value_set->chassis_posture_info.ideal_high = SIT_HIGH; + else if( target_value_set->mode.chassis_high_mode == NORMAL_MODE ) + target_value_set->chassis_posture_info.ideal_high = NORMAL_HIGH + High_Offset - Moving_High_Offset; + else if( target_value_set->mode.chassis_high_mode == HIGH_MODE ) + target_value_set->chassis_posture_info.ideal_high = HIGH_HIGH + High_Offset - Moving_High_Offset; + else if( target_value_set->mode.chassis_high_mode == CHANGING_HIGH ) + { + reduce_high = reduce_high * debug_1; + target_value_set->chassis_posture_info.ideal_high = min(reduce_high,target_value_set->chassis_posture_info.ideal_high); + // target_value_set->chassis_posture_info.ideal_high = + // debug_1 * target_value_set->chassis_posture_info.ideal_high; + } + + if( target_value_set->mode.sport_mode == ABNORMAL_MOVING_MODE || + ( target_value_set->flag_info.suspend_flag_L == 1 && target_value_set->flag_info.suspend_flag_R == 1 ) || + target_value_set->chassis_posture_info.ideal_high == SIT_HIGH || + target_value_set->mode.chassis_balancing_mode == JOINT_REDUCING ) + { + target_value_set->chassis_posture_info.leg_length_L_set = target_value_set->chassis_posture_info.ideal_high; + target_value_set->chassis_posture_info.leg_length_R_set = target_value_set->chassis_posture_info.ideal_high; + } + else + { + target_value_set->chassis_posture_info.foot_roll_angle = + target_value_set->chassis_posture_info.roll_angle + + atan(( target_value_set->chassis_posture_info.leg_length_L - target_value_set->chassis_posture_info.leg_length_R ) / 0.50f ); + + target_value_set->chassis_posture_info.leg_length_L_set = + target_value_set->chassis_posture_info .ideal_high + + 0.25f * arm_sin_f32( target_value_set->chassis_posture_info .foot_roll_angle ) / arm_cos_f32( target_value_set->chassis_posture_info .foot_roll_angle ); + target_value_set->chassis_posture_info.leg_length_R_set = + target_value_set->chassis_posture_info .ideal_high + - 0.25f * arm_sin_f32( target_value_set->chassis_posture_info .foot_roll_angle ) / arm_cos_f32( target_value_set->chassis_posture_info .foot_roll_angle ); + } + +} + +void Chassis_Torque_Calculation(chassis_control_t *bl_ctrl) +{ + + LQR_Data_Update(bl_ctrl); + // -----Roll Balance----- + if( bl_ctrl->flag_info.suspend_flag_R == 1 || bl_ctrl->flag_info.suspend_flag_L == 1 || + bl_ctrl->mode.chassis_high_mode == SIT_MODE ) + { + bl_ctrl->torque_info.joint_roll_torque_R = 0.0f; + bl_ctrl->torque_info.joint_roll_torque_L = 0.0f; + } + else + { + if (bl_ctrl->chassis_posture_info.roll_angle < -roll_angle_deadband) { + rollP = roll_PD[0] * (bl_ctrl->chassis_posture_info.roll_angle_set - (bl_ctrl->chassis_posture_info.roll_angle+roll_angle_deadband)); + } + else if (bl_ctrl->chassis_posture_info.roll_angle > roll_angle_deadband){ + rollP = roll_PD[0] * (bl_ctrl->chassis_posture_info.roll_angle_set - (bl_ctrl->chassis_posture_info.roll_angle-roll_angle_deadband)); + } + else { + rollP = 0.0f; + } + + if (bl_ctrl->chassis_posture_info.roll_gyro < -roll_gyro_deadband) { + rollD = roll_PD[1] * - (bl_ctrl->chassis_posture_info.roll_gyro + roll_gyro_deadband); + } + else if (bl_ctrl->chassis_posture_info.roll_gyro > roll_gyro_deadband){ + rollD = roll_PD[1] * - (bl_ctrl->chassis_posture_info.roll_gyro - roll_gyro_deadband); + } + else { + rollD = 0.0f; + } + bl_ctrl->torque_info.joint_roll_torque_R = rollP + rollD; + bl_ctrl->torque_info.joint_roll_torque_L = -bl_ctrl->torque_info.joint_roll_torque_R; + } + + + // -----During turns: prevent displacement of two legs----- + bl_ctrl->torque_info.joint_prevent_splits_torque_L = + coordinate_PD[0] * (bl_ctrl->chassis_posture_info.leg_angle_L - bl_ctrl->chassis_posture_info.leg_angle_R) + + coordinate_PD[1] * (bl_ctrl->chassis_posture_info.leg_gyro_L - bl_ctrl->chassis_posture_info.leg_gyro_R); + + bl_ctrl->torque_info.joint_prevent_splits_torque_R = -bl_ctrl->torque_info.joint_prevent_splits_torque_L; + + if( bl_ctrl->mode.jumping_stage == EXTENDING_LEGS || + bl_ctrl->mode.jumping_stage == CONSTACTING_LEGS_2 ) + { + bl_ctrl->torque_info.joint_stand_torque_L = + + jump_stand_PD[0] * ( bl_ctrl->chassis_posture_info.leg_length_L_set - bl_ctrl->chassis_posture_info.leg_length_L ) + + jump_stand_PD[1] * ( 0 - bl_ctrl->chassis_posture_info.leg_dlength_L ); + + bl_ctrl->torque_info.joint_stand_torque_R = + + jump_stand_PD[0] * ( bl_ctrl->chassis_posture_info.leg_length_R_set - bl_ctrl->chassis_posture_info.leg_length_R ) + + jump_stand_PD[1] * ( 0 - bl_ctrl->chassis_posture_info.leg_dlength_R ); + } + else if( bl_ctrl->mode.sport_mode == ABNORMAL_MOVING_MODE ) + { + bl_ctrl->torque_info.joint_stand_torque_L = + + suspend_stand_PD[0] * ( bl_ctrl->chassis_posture_info.leg_length_L_set - bl_ctrl->chassis_posture_info.leg_length_L ) + + suspend_stand_PD[1] * ( 0.0f - bl_ctrl->chassis_posture_info.leg_dlength_L ); + bl_ctrl->torque_info.joint_stand_torque_R = + + suspend_stand_PD[0] * ( bl_ctrl->chassis_posture_info.leg_length_R_set - bl_ctrl->chassis_posture_info.leg_length_R ) + + suspend_stand_PD[1] * ( 0.0f - bl_ctrl->chassis_posture_info.leg_dlength_R ); + } + else if( bl_ctrl->flag_info.suspend_flag_R == 1 || bl_ctrl->flag_info.suspend_flag_L == 1 ) + { + if( bl_ctrl->flag_info.suspend_flag_L == 1 ) + { + bl_ctrl->torque_info.joint_stand_torque_L = + + suspend_stand_PD[0] * ( bl_ctrl->chassis_posture_info.leg_length_L_set - bl_ctrl->chassis_posture_info.leg_length_L ) + + suspend_stand_PD[1] * ( 0.0f - bl_ctrl->chassis_posture_info.leg_dlength_L ); + } + else{ + bl_ctrl->torque_info.joint_stand_torque_L = + FEED_f + + stand_PD[0] * ( bl_ctrl->chassis_posture_info.leg_length_L_set - bl_ctrl->chassis_posture_info.leg_length_L ) + + stand_PD[1] * ( 0 - bl_ctrl->chassis_posture_info.leg_dlength_L ); + } + + if( bl_ctrl->flag_info.suspend_flag_R == 1 ) + { + bl_ctrl->torque_info.joint_stand_torque_R = + + suspend_stand_PD[0] * ( bl_ctrl->chassis_posture_info.leg_length_R_set - bl_ctrl->chassis_posture_info.leg_length_R ) + + suspend_stand_PD[1] * ( 0.0f - bl_ctrl->chassis_posture_info.leg_dlength_R ); + } else { + bl_ctrl->torque_info.joint_stand_torque_R = + FEED_f + + stand_PD[0] * ( bl_ctrl->chassis_posture_info.leg_length_R_set - bl_ctrl->chassis_posture_info.leg_length_R ) + + stand_PD[1] * ( 0 - bl_ctrl->chassis_posture_info.leg_dlength_R ); + } + } + else if( bl_ctrl->mode.chassis_balancing_mode == JOINT_REDUCING ) + { + + bl_ctrl->torque_info.joint_stand_torque_L = + + suspend_stand_PD[0] * ( bl_ctrl->chassis_posture_info.leg_length_L_set - bl_ctrl->chassis_posture_info.leg_length_L ) + + suspend_stand_PD[1] * ( 0.0f - bl_ctrl->chassis_posture_info.leg_dlength_L ); + + bl_ctrl->torque_info.joint_stand_torque_R = + + suspend_stand_PD[0] * ( bl_ctrl->chassis_posture_info.leg_length_R_set - bl_ctrl->chassis_posture_info.leg_length_R ) + + suspend_stand_PD[1] * ( 0.0f - bl_ctrl->chassis_posture_info.leg_dlength_R ); + } + else + { + bl_ctrl->torque_info.joint_stand_torque_L = FEED_f; + bl_ctrl->torque_info.joint_stand_torque_L += stand_PD[0] * ( bl_ctrl->chassis_posture_info.leg_length_L_set - bl_ctrl->chassis_posture_info.leg_length_L ); + if (bl_ctrl->chassis_posture_info.leg_dlength_L > leg_dlength_deadband){ + bl_ctrl->torque_info.joint_stand_torque_L += stand_PD[1] * ( 0 - (bl_ctrl->chassis_posture_info.leg_dlength_L-leg_dlength_deadband) ); + } + else if (bl_ctrl->chassis_posture_info.leg_dlength_L < -leg_dlength_deadband){ + bl_ctrl->torque_info.joint_stand_torque_L += stand_PD[1] * ( 0 - (bl_ctrl->chassis_posture_info.leg_dlength_L+leg_dlength_deadband)); + } + else { + bl_ctrl->torque_info.joint_stand_torque_L += 0.0f; + } + bl_ctrl->torque_info.joint_stand_torque_R = FEED_f; + bl_ctrl->torque_info.joint_stand_torque_R += stand_PD[0] * ( bl_ctrl->chassis_posture_info.leg_length_R_set - bl_ctrl->chassis_posture_info.leg_length_R ); + if (bl_ctrl->chassis_posture_info.leg_dlength_R > leg_dlength_deadband){ + bl_ctrl->torque_info.joint_stand_torque_R += stand_PD[1] * ( 0 - (bl_ctrl->chassis_posture_info.leg_dlength_R-leg_dlength_deadband) ); + } + else if (bl_ctrl->chassis_posture_info.leg_dlength_R < -leg_dlength_deadband){ + bl_ctrl->torque_info.joint_stand_torque_R += stand_PD[1] * ( 0 - (bl_ctrl->chassis_posture_info.leg_dlength_R+leg_dlength_deadband)); + } + else { + bl_ctrl->torque_info.joint_stand_torque_R += 0.0f; + } + + } + // --------------------- joint motor calucaltion --------------------- + + + // TODDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDO + if( bl_ctrl->mode.jumping_stage == CONSTACTING_LEGS_2 ) + { + bl_ctrl->torque_info.joint_balancing_torque_L = + -suspend_LQR[0][0] * ( 0 - bl_ctrl->chassis_posture_info.leg_angle_L ) + -suspend_LQR[0][1] * ( 0 - bl_ctrl->chassis_posture_info.leg_gyro_L ); + bl_ctrl->torque_info.joint_balancing_torque_R = + -suspend_LQR[0][0] * ( 0 - bl_ctrl->chassis_posture_info.leg_angle_R ) + -suspend_LQR[0][1] * ( 0 - bl_ctrl->chassis_posture_info.leg_gyro_R ); + + bl_ctrl->torque_info.joint_moving_torque_L = 0.0f; + bl_ctrl->torque_info.joint_moving_torque_R = 0.0f; + } + else + { + if( bl_ctrl->mode.sport_mode == ABNORMAL_MOVING_MODE || bl_ctrl->mode.sport_mode == TK_MODE ) + { + bl_ctrl->torque_info.joint_balancing_torque_L = + bl_ctrl->torque_info.joint_moving_torque_L = + bl_ctrl->torque_info.joint_balancing_torque_R = + bl_ctrl->torque_info.joint_moving_torque_R = 0; + } + else + { + if( bl_ctrl->mode.chassis_high_mode == SIT_MODE || + bl_ctrl->mode.chassis_balancing_mode == JOINT_REDUCING ) + { + bl_ctrl->torque_info.joint_balancing_torque_L = + bl_ctrl->torque_info.joint_moving_torque_L = + bl_ctrl->torque_info.joint_balancing_torque_R = + bl_ctrl->torque_info.joint_moving_torque_R = 0; + } + else + { + + bl_ctrl->torque_info.joint_balancing_torque_L = ( + -LQR[2][4] * ( bl_ctrl->chassis_posture_info.leg_angle_L_set - bl_ctrl->chassis_posture_info.leg_angle_L ) + -LQR[2][5] * ( 0.0f - bl_ctrl->chassis_posture_info.leg_gyro_L ) + -LQR[2][8] * ( bl_ctrl->chassis_posture_info.pitch_angle_set - bl_ctrl->chassis_posture_info.pitch_angle ) + -LQR[2][9] * ( bl_ctrl->chassis_posture_info.pitch_gyro_set - bl_ctrl->chassis_posture_info.pitch_gyro ) ); + bl_ctrl->torque_info.joint_moving_torque_L = ( + +LQR[2][0] * ( bl_ctrl->chassis_posture_info.foot_distance_set - bl_ctrl->chassis_posture_info.foot_distance_K + NORMAL_MODE_WEIGHT_DISTANCE_OFFSET) + +LQR[2][1] * ( bl_ctrl->chassis_posture_info.foot_speed_set - bl_ctrl->chassis_posture_info.foot_speed_K ) + +LQR[2][2] * ( bl_ctrl->chassis_posture_info.yaw_angle_sett - bl_ctrl->chassis_posture_info.yaw_angle ) + +LQR[2][3] * ( bl_ctrl->chassis_posture_info.yaw_gyro_set - bl_ctrl->chassis_posture_info.yaw_gyro ) + ); + + bl_ctrl->torque_info.joint_balancing_torque_R = ( + -LQR[3][6] * ( bl_ctrl->chassis_posture_info.leg_angle_R_set - bl_ctrl->chassis_posture_info.leg_angle_R ) + -LQR[3][7] * ( 0.0f - bl_ctrl->chassis_posture_info.leg_gyro_R ) + -LQR[3][8] * ( bl_ctrl->chassis_posture_info.pitch_angle_set - bl_ctrl->chassis_posture_info.pitch_angle ) + -LQR[3][9] * ( bl_ctrl->chassis_posture_info.pitch_gyro_set - bl_ctrl->chassis_posture_info.pitch_gyro ) ); + bl_ctrl->torque_info.joint_moving_torque_R = ( + +LQR[3][0] * ( bl_ctrl->chassis_posture_info.foot_distance_set - bl_ctrl->chassis_posture_info.foot_distance_K + NORMAL_MODE_WEIGHT_DISTANCE_OFFSET) + +LQR[3][1] * ( bl_ctrl->chassis_posture_info.foot_speed_set - bl_ctrl->chassis_posture_info.foot_speed_K ) + +LQR[3][2] * ( bl_ctrl->chassis_posture_info.yaw_angle_sett - bl_ctrl->chassis_posture_info.yaw_angle ) + +LQR[3][3] * ( bl_ctrl->chassis_posture_info.yaw_gyro_set - bl_ctrl->chassis_posture_info.yaw_gyro ) + ); + } + } + } + + // --------------------- Foot motor LQR --------------------- + if ( bl_ctrl->mode.sport_mode == TK_MODE ) { + bl_ctrl->torque_info.foot_balancing_torque_L = 0.0f; + bl_ctrl->torque_info.foot_balancing_torque_R = 0.0f; + bl_ctrl->torque_info.foot_moving_torque_L = (int) ( + -TK_x_p*( bl_ctrl->chassis_posture_info.foot_distance_set - bl_ctrl->chassis_posture_info.foot_speed_K ) + -TK_y_p*( bl_ctrl->chassis_posture_info.foot_speed_set - bl_ctrl->chassis_posture_info.yaw_angle ) + -TK_y_d*( 0.0f - bl_ctrl->chassis_posture_info.yaw_gyro ) + )* TORQ_K; + bl_ctrl->torque_info.foot_moving_torque_R = (int)-( + -TK_x_p*( bl_ctrl->chassis_posture_info.foot_distance_set - bl_ctrl->chassis_posture_info.foot_speed_K ) + +TK_y_p*( bl_ctrl->chassis_posture_info.foot_speed_set - bl_ctrl->chassis_posture_info.yaw_angle ) + +TK_y_d*( 0.0f - bl_ctrl->chassis_posture_info.yaw_gyro ) + )* TORQ_K; + } + else { + bl_ctrl->torque_info.foot_balancing_torque_L = (int) ( + -LQR[0][4]*( bl_ctrl->chassis_posture_info.leg_angle_L_set - bl_ctrl->chassis_posture_info.leg_angle_L) + -LQR[0][5]*( 0.0f - bl_ctrl->chassis_posture_info.leg_gyro_L) + +LQR[0][8]*( bl_ctrl->chassis_posture_info.pitch_angle_set - bl_ctrl->chassis_posture_info.pitch_angle) + +LQR[0][9]*( bl_ctrl->chassis_posture_info.pitch_gyro_set - bl_ctrl->chassis_posture_info.pitch_gyro) + )* TORQ_K; + + bl_ctrl->torque_info.foot_moving_torque_L = (int) ( + -LQR[0][0]*( bl_ctrl->chassis_posture_info.foot_distance_set - bl_ctrl->chassis_posture_info.foot_distance_K + NORMAL_MODE_WEIGHT_DISTANCE_OFFSET) + -LQR[0][1]*( bl_ctrl->chassis_posture_info.foot_speed_set - bl_ctrl->chassis_posture_info.foot_speed_K) + -LQR[0][2]*( bl_ctrl->chassis_posture_info.yaw_angle_sett - bl_ctrl->chassis_posture_info.yaw_angle) + -LQR[0][3]*( bl_ctrl->chassis_posture_info.yaw_gyro_set - bl_ctrl->chassis_posture_info.yaw_gyro ) + )* TORQ_K; + + bl_ctrl->torque_info.foot_balancing_torque_R = (int) -( + -LQR[1][6]*( bl_ctrl->chassis_posture_info.leg_angle_R_set - bl_ctrl->chassis_posture_info.leg_angle_R) + -LQR[1][7]*( 0.0f - bl_ctrl->chassis_posture_info.leg_gyro_R) + +LQR[1][8]*( bl_ctrl->chassis_posture_info.pitch_angle_set - bl_ctrl->chassis_posture_info.pitch_angle) + +LQR[1][9]*( bl_ctrl->chassis_posture_info.pitch_gyro_set - bl_ctrl->chassis_posture_info.pitch_gyro) + )* TORQ_K; + + bl_ctrl->torque_info.foot_moving_torque_R = (int) -( + -LQR[1][0]*( bl_ctrl->chassis_posture_info.foot_distance_set - bl_ctrl->chassis_posture_info.foot_distance_K + NORMAL_MODE_WEIGHT_DISTANCE_OFFSET) + -LQR[1][1]*( bl_ctrl->chassis_posture_info.foot_speed_set - bl_ctrl->chassis_posture_info.foot_speed_K) + +LQR[1][2]*( bl_ctrl->chassis_posture_info.yaw_angle_sett - bl_ctrl->chassis_posture_info.yaw_angle) + +LQR[1][3]*( bl_ctrl->chassis_posture_info.yaw_gyro_set - bl_ctrl->chassis_posture_info.yaw_gyro ) + )* TORQ_K; + } + + if( bl_ctrl->flag_info.suspend_flag_R == 1 ) + { + bl_ctrl->torque_info.joint_balancing_torque_R = + -suspend_LQR[0][0] * ( 0.0f - bl_ctrl->chassis_posture_info.leg_angle_R ) + -suspend_LQR[0][1] * ( 0 - bl_ctrl->chassis_posture_info.leg_gyro_R ); + + bl_ctrl->torque_info.foot_moving_torque_R = + -suspend_foot_speed_p * ( 0.0f - bl_ctrl->foot_motor_R.speed ); + + bl_ctrl->torque_info.joint_moving_torque_R = 0.0f; + bl_ctrl->torque_info.foot_balancing_torque_R = 0.0f; + } + if( bl_ctrl->flag_info.suspend_flag_L == 1 ) + { + bl_ctrl->torque_info.joint_balancing_torque_L = + -suspend_LQR[0][0] * ( 0.0f - bl_ctrl->chassis_posture_info.leg_angle_L ) + -suspend_LQR[0][1] * ( 0 - bl_ctrl->chassis_posture_info.leg_gyro_L ); + bl_ctrl->torque_info.foot_moving_torque_L = + +suspend_foot_speed_p * ( 0.0f - bl_ctrl->foot_motor_L.speed ); + + bl_ctrl->torque_info.joint_moving_torque_L = 0.0f; + bl_ctrl->torque_info.foot_balancing_torque_L = 0.0f; + } + + + // LimitMax( bl_ctrl->torque_info.foot_moving_torque_L, MAX_ACCL ); + // LimitMax( bl_ctrl->torque_info.foot_moving_torque_R, MAX_ACCL ); + LimitMax( bl_ctrl->torque_info.joint_moving_torque_L, MAX_ACCL_JOINT ); + LimitMax( bl_ctrl->torque_info.joint_moving_torque_R, MAX_ACCL_JOINT ); + +} + +void Chassis_Torque_Combine(chassis_control_t *bl_ctrl) +{ + bl_ctrl->mapping_info .J1_L = VMC_solve_J1(bl_ctrl->chassis_posture_info .leg_angle_L ,bl_ctrl->chassis_posture_info .leg_length_L ); + bl_ctrl->mapping_info .J2_L = VMC_solve_J2(bl_ctrl->chassis_posture_info .leg_angle_L ,bl_ctrl->chassis_posture_info .leg_length_L); + bl_ctrl->mapping_info .J3_L = VMC_solve_J3(bl_ctrl->chassis_posture_info .leg_angle_L ,bl_ctrl->chassis_posture_info .leg_length_L); + bl_ctrl->mapping_info .J4_L = VMC_solve_J4(bl_ctrl->chassis_posture_info .leg_angle_L ,bl_ctrl->chassis_posture_info .leg_length_L ); + bl_ctrl->mapping_info .J1_R = VMC_solve_J1(bl_ctrl->chassis_posture_info .leg_angle_R ,bl_ctrl->chassis_posture_info .leg_length_R ); + bl_ctrl->mapping_info .J2_R = VMC_solve_J2(bl_ctrl->chassis_posture_info .leg_angle_R ,bl_ctrl->chassis_posture_info .leg_length_R); + bl_ctrl->mapping_info .J3_R = VMC_solve_J3(bl_ctrl->chassis_posture_info .leg_angle_R ,bl_ctrl->chassis_posture_info .leg_length_R); + bl_ctrl->mapping_info .J4_R = VMC_solve_J4(bl_ctrl->chassis_posture_info .leg_angle_R ,bl_ctrl->chassis_posture_info .leg_length_R ); + + bl_ctrl->torque_info.foot_horizontal_torque_L = + bl_ctrl->torque_info.foot_balancing_torque_L + bl_ctrl->torque_info.foot_moving_torque_L; + bl_ctrl->torque_info.foot_horizontal_torque_R = + bl_ctrl->torque_info.foot_balancing_torque_R + bl_ctrl->torque_info.foot_moving_torque_R; + + bl_ctrl->foot_motor_L.torque_out = bl_ctrl->torque_info.foot_horizontal_torque_L; + bl_ctrl->foot_motor_R.torque_out = bl_ctrl->torque_info.foot_horizontal_torque_R; + + LimitMax(bl_ctrl->foot_motor_L.torque_out,16383); + LimitMax(bl_ctrl->foot_motor_R.torque_out,16383); + + bl_ctrl->torque_info.joint_horizontal_torque_L = + bl_ctrl->torque_info.joint_balancing_torque_L + bl_ctrl->torque_info.joint_moving_torque_L + bl_ctrl->torque_info.joint_prevent_splits_torque_L; + bl_ctrl->torque_info.joint_horizontal_torque_R = + bl_ctrl->torque_info.joint_balancing_torque_R + bl_ctrl->torque_info.joint_moving_torque_R + bl_ctrl->torque_info.joint_prevent_splits_torque_R; + + bl_ctrl->torque_info.joint_vertical_torque_L = + bl_ctrl->torque_info.joint_stand_torque_L + bl_ctrl->torque_info.joint_roll_torque_L; + bl_ctrl->torque_info.joint_vertical_torque_R = + bl_ctrl->torque_info.joint_stand_torque_R + bl_ctrl->torque_info.joint_roll_torque_R; + + bl_ctrl->torque_info.joint_horizontal_torque_temp1_L = + (bl_ctrl->torque_info.joint_horizontal_torque_L) * bl_ctrl->mapping_info .J3_L ; + bl_ctrl->torque_info.joint_horizontal_torque_temp2_L = + (bl_ctrl->torque_info.joint_horizontal_torque_L) * bl_ctrl->mapping_info .J4_L ; + bl_ctrl->torque_info.joint_horizontal_torque_temp1_R = + (bl_ctrl->torque_info.joint_horizontal_torque_R) * bl_ctrl->mapping_info .J3_R ; + bl_ctrl->torque_info.joint_horizontal_torque_temp2_R = + (bl_ctrl->torque_info.joint_horizontal_torque_R) * bl_ctrl->mapping_info .J4_R ; + + bl_ctrl->torque_info.joint_vertical_torque_temp1_L = + (bl_ctrl->torque_info.joint_vertical_torque_L) * bl_ctrl->mapping_info .J1_L; + bl_ctrl->torque_info.joint_vertical_torque_temp2_L = + (bl_ctrl->torque_info.joint_vertical_torque_L) * bl_ctrl->mapping_info .J2_L; + bl_ctrl->torque_info.joint_vertical_torque_temp1_R = + (bl_ctrl->torque_info.joint_vertical_torque_R) * bl_ctrl->mapping_info .J1_R; + bl_ctrl->torque_info.joint_vertical_torque_temp2_R = + (bl_ctrl->torque_info.joint_vertical_torque_R) * bl_ctrl->mapping_info .J2_R; + + /****************************************/ + + fp32 MAX_balance = 2000.0f; + + LimitMax(bl_ctrl->torque_info.joint_horizontal_torque_temp1_L,MAX_balance); + LimitMax(bl_ctrl->torque_info.joint_horizontal_torque_temp2_L,MAX_balance); + LimitMax(bl_ctrl->torque_info.joint_horizontal_torque_temp1_R,MAX_balance); + LimitMax(bl_ctrl->torque_info.joint_horizontal_torque_temp2_R,MAX_balance); + LimitMax(bl_ctrl->torque_info.joint_vertical_torque_temp1_L,15); + LimitMax(bl_ctrl->torque_info.joint_vertical_torque_temp2_L,15); + LimitMax(bl_ctrl->torque_info.joint_vertical_torque_temp1_R,15); + LimitMax(bl_ctrl->torque_info.joint_vertical_torque_temp2_R,15); + + // 分配到四�?关节电机 + bl_ctrl->joint_motor_1.torque_out = + bl_ctrl->torque_info.joint_horizontal_torque_temp1_L + bl_ctrl->torque_info.joint_vertical_torque_temp1_L; + bl_ctrl->joint_motor_2.torque_out = + bl_ctrl->torque_info.joint_horizontal_torque_temp2_L + bl_ctrl->torque_info.joint_vertical_torque_temp2_L; + bl_ctrl->joint_motor_3.torque_out = - bl_ctrl->torque_info.joint_horizontal_torque_temp1_R + bl_ctrl->torque_info.joint_vertical_torque_temp1_R; + bl_ctrl->joint_motor_4.torque_out = - bl_ctrl->torque_info.joint_horizontal_torque_temp2_R + bl_ctrl->torque_info.joint_vertical_torque_temp2_R; + + if( ABS(bl_ctrl->joint_motor_1.motor_measure->ecd) <= MOTOR_POS_UPPER_BOUND ) + { + bl_ctrl->joint_motor_1.max_torque = LIMITED_TORQUE; + bl_ctrl->joint_motor_1.min_torque = -1.0f * UNLIMITED_TORQUE; + } + else if( ABS(bl_ctrl->joint_motor_1.motor_measure->ecd) >= MOTOR_POS_LOWER_BOUND ) + { + bl_ctrl->joint_motor_1.max_torque = UNLIMITED_TORQUE; + bl_ctrl->joint_motor_1.min_torque = -1.0f * LIMITED_TORQUE; + } + else + { + bl_ctrl->joint_motor_1.max_torque = UNLIMITED_TORQUE; + bl_ctrl->joint_motor_1.min_torque = -1.0f * UNLIMITED_TORQUE; + } + + if( ABS(bl_ctrl->joint_motor_3.motor_measure->ecd) <= MOTOR_POS_UPPER_BOUND ) + { + bl_ctrl->joint_motor_3.max_torque = LIMITED_TORQUE; + bl_ctrl->joint_motor_3.min_torque = -1.0f * UNLIMITED_TORQUE; + } + else if( ABS(bl_ctrl->joint_motor_3.motor_measure->ecd) >= MOTOR_POS_LOWER_BOUND ) + { + bl_ctrl->joint_motor_3.max_torque = UNLIMITED_TORQUE; + bl_ctrl->joint_motor_3.min_torque = -1.0f * LIMITED_TORQUE; + } + else + { + bl_ctrl->joint_motor_3.max_torque = UNLIMITED_TORQUE; + bl_ctrl->joint_motor_3.min_torque = -1.0f * UNLIMITED_TORQUE; + } + + if( ABS(bl_ctrl->joint_motor_2.motor_measure->ecd) <= MOTOR_POS_UPPER_BOUND ) + { + bl_ctrl->joint_motor_2.max_torque = UNLIMITED_TORQUE; + bl_ctrl->joint_motor_2.min_torque = -1.0f * LIMITED_TORQUE; + } + else if( ABS(bl_ctrl->joint_motor_2.motor_measure->ecd) >= MOTOR_POS_LOWER_BOUND ) + { + bl_ctrl->joint_motor_2.max_torque = LIMITED_TORQUE; + bl_ctrl->joint_motor_2.min_torque = -1.0f * UNLIMITED_TORQUE; + } + else + { + bl_ctrl->joint_motor_2.max_torque = UNLIMITED_TORQUE; + bl_ctrl->joint_motor_2.min_torque = -1.0f * UNLIMITED_TORQUE; + } + + if( ABS(bl_ctrl->joint_motor_4.motor_measure->ecd) <= MOTOR_POS_UPPER_BOUND ) + { + bl_ctrl->joint_motor_4.max_torque = UNLIMITED_TORQUE; + bl_ctrl->joint_motor_4.min_torque = -1.0f * LIMITED_TORQUE; + } + else if( ABS(bl_ctrl->joint_motor_4.motor_measure->ecd) >= MOTOR_POS_LOWER_BOUND ) + { + bl_ctrl->joint_motor_4.max_torque = LIMITED_TORQUE; + bl_ctrl->joint_motor_4.min_torque = -1.0f * UNLIMITED_TORQUE; + } + else + { + bl_ctrl->joint_motor_4.max_torque = UNLIMITED_TORQUE; + bl_ctrl->joint_motor_4.min_torque = -1.0f * UNLIMITED_TORQUE; + } + + LimitOutput( bl_ctrl->joint_motor_1.torque_out, bl_ctrl->joint_motor_1.min_torque, bl_ctrl->joint_motor_1.max_torque); + LimitOutput( bl_ctrl->joint_motor_2.torque_out, bl_ctrl->joint_motor_2.min_torque, bl_ctrl->joint_motor_2.max_torque); + LimitOutput( bl_ctrl->joint_motor_3.torque_out, bl_ctrl->joint_motor_3.min_torque, bl_ctrl->joint_motor_3.max_torque); + LimitOutput( bl_ctrl->joint_motor_4.torque_out, bl_ctrl->joint_motor_4.min_torque, bl_ctrl->joint_motor_4.max_torque); +} + +void Motor_CMD_Send(chassis_control_t *CMD_Send) +{ + if( CMD_Send->joint_motor_1.motor_mode == MOTOR_FORCE ) + CAN_HT_CMD( 0x01, CMD_Send->joint_motor_1.torque_out ); + else + CAN_HT_CMD( 0x01, 0.0 ); + if( CMD_Send->joint_motor_2.motor_mode == MOTOR_FORCE ) + CAN_HT_CMD( 0x02, CMD_Send->joint_motor_2.torque_out ); + else + CAN_HT_CMD( 0x02, 0.0 ); + + + if( CMD_Send->foot_motor_R.motor_mode != MOTOR_FORCE ) + CMD_Send->foot_motor_R .torque_out = 0.0f; + if( CMD_Send->foot_motor_L.motor_mode != MOTOR_FORCE ) + CMD_Send->foot_motor_L .torque_out = 0.0f; + CAN_BM_CONTROL_CMD( CMD_Send->foot_motor_L.torque_out, CMD_Send->foot_motor_R.torque_out ); + + vTaskDelay(1); + + if( CMD_Send->joint_motor_3.motor_mode == MOTOR_FORCE ) + CAN_HT_CMD( 0x03, CMD_Send->joint_motor_3.torque_out ); + else + CAN_HT_CMD( 0x03, 0.0 ); + + + if( CMD_Send->joint_motor_4.motor_mode == MOTOR_FORCE ) + CAN_HT_CMD( 0x04, CMD_Send->joint_motor_4.torque_out ); + else + CAN_HT_CMD( 0x04, 0.0 ); +} + +void Joint_Motor_to_Init_Pos() +{ + int Init_Time = 0; + while( Init_Time < 300 ) + { + CAN_HT_CMD( 0x01, 1.0 ); + CAN_HT_CMD( 0x02, -1.0 ); + CAN_HT_CMD( 0x03, 1.0 ); + vTaskDelay(1); + CAN_HT_CMD( 0x04, -1.0 ); + vTaskDelay(1); + Init_Time++; + } +} + +void HT_Motor_zero_set(void) +{ + uint8_t tx_buff[8]; + for( int i = 0; i < 7; i++ ) + tx_buff[i] = 0xFF; + tx_buff[7] = 0xfc; + + // ENABLE_CHASSIS + CAN_CMD_HT_Enable( 0x01, tx_buff ); + vTaskDelay(50); + CAN_CMD_HT_Enable( 0x02, tx_buff ); + vTaskDelay(50); + CAN_CMD_HT_Enable( 0x03, tx_buff ); + vTaskDelay(50); + CAN_CMD_HT_Enable( 0x04, tx_buff ); + vTaskDelay(50); + // �?到限�? + Joint_Motor_to_Init_Pos(); + // Set zero init point + tx_buff[7] = 0xfe; + + CAN_CMD_HT_Enable( 0x01, tx_buff ); + vTaskDelay(50); + CAN_CMD_HT_Enable( 0x02, tx_buff ); + vTaskDelay(50); + CAN_CMD_HT_Enable( 0x03, tx_buff ); + vTaskDelay(50); + CAN_CMD_HT_Enable( 0x04, tx_buff ); + + vTaskDelay(50); + +} + +void Motor_Zero_CMD_Send(void) +{ + CAN_HT_CMD( 0x01, 0.0 ); + vTaskDelay(1); + CAN_HT_CMD( 0x02, 0.0 ); + vTaskDelay(1); + CAN_HT_CMD( 0x03, 0.0 ); + vTaskDelay(1); + CAN_HT_CMD( 0x04, 0.0 ); + vTaskDelay(1); + + // CAN_BM_ENABLE_CMD(); + // vTaskDelay(1); + + // CAN_BM_MODE_SET_CMD(); + // vTaskDelay(2); + + // CAN_BM_CURRENT_MODE_CMD(); + // vTaskDelay(1); + +} diff --git a/utils/香港大学轮腿电控及建模开源-3/Chassis_Task.h b/utils/香港大学轮腿电控及建模开源-3/Chassis_Task.h new file mode 100644 index 0000000..2d33123 --- /dev/null +++ b/utils/香港大学轮腿电控及建模开源-3/Chassis_Task.h @@ -0,0 +1,299 @@ +#ifndef _CHASSIS_TASK +#define _CHASSIS_TASK + +#include "main.h" +#include "struct_typedef.h" +#include "pid.h" +#include "bsp_can.h" +// ------------- Limit info ------------- +#define MAX_ACCL 13000.0f +#define MAX_ACCL_JOINT 15.0f +#define MAX_FOOT_OUTPUT 2048 + +// ------------- Target value info ------------- +#define SIT_MODE_HEIGHT_SET 0.18f +#define NORMAL_MODE_HEIGHT_SET 0.10f +#define HIGH_MODE_HEIGHT_SET 0.27f +#define EXTREMELY_HIGH_MODE_HEIGHT_SET 0.30f + +// ------------- Mech info ------------- +#define L1 0.15f +#define L2 0.25f +#define L3 0.25f +#define L4 0.15f +#define L5 0.1f + +#define WHEEL_PERIMETER 0.56547 +#define WHEEL_RADIUS 0.09f +#define LEG_OFFSET 0.3790855135f // 水平位置到上限位的夹角 +#define LOWER_SUPPORT_FORCE_FOR_JUMP 5.0f +#define LOWER_SUPPORT_FORCE 0.0f +#define MOVE_LOWER_BOUND 0.3f +#define EXIT_PITCH_ANGLE 0.1f +#define DANGER_PITCH_ANGLE 0.25f + +#define FEED_f 7.5f +#define FEED_f_1 3.5f + +#define NORMAL_MODE_WEIGHT_DISTANCE_OFFSET -0.0f + +#define MOTOR_POS_UPPER_BOUND 0.05f +#define MOTOR_POS_LOWER_BOUND 1.4f +#define LIMITED_TORQUE 0.5f +#define UNLIMITED_TORQUE 200.0f + +// ------------- Time info ------------- +#define CHASSIS_TASK_INIT_TIME 500 +#define TASK_RUN_TIME 0.002f + +// ------------- Transfer info ------------- +#define MOTOR_ECD_TO_RAD 0.00019174779 // 2*PI / 32767 +#define HALF_ECD_RANGE 14383 +#define HALF_POSITION_RANGE 3.0f +// #define CC 0.00512f +// #define CC 1/494.0f +#define TORQ_K 494.483818182 +// ------------- Math info ------------- +#define PI2 6.28318530717959f +#define PI 3.14159265358979f +#define PI_2 1.57079632679489f +#define PI_4 0.78539816339744f + + + +typedef enum +{ + ENABLE_CHASSIS = 0, + DISABLE_CHASSIS, +} chassis_mode_e; + +typedef enum +{ + NO_FORCE, + FOOT_LAUNCHING, + JOINT_LAUNCHING, + BALANCING_READY, + JOINT_REDUCING, +} chassis_balancing_mode_e; + +typedef enum +{ + NONE, + NORMAL_MOVING_MODE, + ABNORMAL_MOVING_MODE, + JUMPING_MODE, + CAP_MODE, + FLY_MODE, + TK_MODE, +} sport_mode_e; + +typedef enum +{ + READY_TO_JUMP, + PREPARING_LANDING, + PREPARING_STAND_JUMPING, + CONSTACTING_LEGS, + EXTENDING_LEGS, + CONSTACTING_LEGS_2, + FINISHED, +} jumping_stage_e; + +typedef enum +{ + NOT_DEFINE, + STANDING_JUMP, + MOVING_JUMP, +} jumping_mode_e; + +typedef enum +{ + SIT_MODE = 0, + NORMAL_MODE, + HIGH_MODE, + EXTREMELY_HIGH_MODE, + CHANGING_HIGH, +} chassis_high_mode_e; + +typedef enum +{ + MOTOR_NO_FORCE = 0, + MOTOR_FORCE, +} chassis_motor_mode_e; + +typedef enum +{ + ON_GROUND = 0, + OFF_GROUND = 1, +} suspend_flag_e; + +typedef struct +{ + chassis_mode_e chassis_mode, last_chassis_mode; + chassis_balancing_mode_e chassis_balancing_mode, last_chassis_balancing_mode; + sport_mode_e sport_mode, last_sport_mode; + + jumping_mode_e jumping_mode, last_jumping_mode; + jumping_stage_e jumping_stage, last_jumping_stage; + + chassis_high_mode_e chassis_high_mode, last_chassis_high_mode; + +} mode_t; + +typedef struct +{ + const fp32 *chassis_INS_angle_point; + const fp32 *chassis_INS_gyro_point; + const fp32 *chassis_INS_accel_point; + fp32 yaw_angle, pitch_angle, roll_angle; + fp32 yaw_gyro, pitch_gyro, roll_gyro; + fp32 yaw_accel, pitch_accel, roll_accel; + + fp32 yaw_angle_sett, pitch_angle_set, roll_angle_set; + fp32 yaw_gyro_set, pitch_gyro_set, roll_gyro_set; + + fp32 ideal_high; + fp32 leg_length_L, last_leg_length_L, leg_length_L_set; + fp32 leg_length_R, last_leg_length_R, leg_length_R_set; + fp32 leg_dlength_L; + fp32 leg_dlength_R; + + fp32 foot_roll_angle; + fp32 leg_angle_L, last_leg_angle_L, leg_angle_L_set; + fp32 leg_angle_R, last_leg_angle_R, leg_angle_R_set; + fp32 leg_gyro_L, leg_gyro_R; + + fp32 foot_distance, foot_distance_K, foot_distance_set; + fp32 foot_speed, foot_speed_K, foot_speed_set; + + fp32 supportive_force_L, supportive_force_R; + +} chassis_posture_info_t; + +typedef struct +{ + // -------- horizontal force -------- + fp32 joint_balancing_torque_L, joint_balancing_torque_R; + fp32 foot_balancing_torque_L, foot_balancing_torque_R; + + fp32 foot_moving_torque_L, foot_moving_torque_R; + fp32 joint_moving_torque_L, joint_moving_torque_R; + + fp32 joint_prevent_splits_torque_L, joint_prevent_splits_torque_R; + + fp32 joint_horizontal_torque_L, joint_horizontal_torque_R; + fp32 foot_horizontal_torque_L, foot_horizontal_torque_R; + + fp32 joint_horizontal_torque_temp1_R, joint_horizontal_torque_temp2_R; + fp32 joint_horizontal_torque_temp1_L, joint_horizontal_torque_temp2_L; + + fp32 yaw_torque; + + // -------- vertical force -------- + fp32 joint_roll_torque_L, joint_roll_torque_R; + fp32 joint_stand_torque_L, joint_stand_torque_R; + + fp32 joint_vertical_torque_L, joint_vertical_torque_R; + fp32 joint_real_vertical_torque_L, joint_real_vertical_torque_R; + + fp32 joint_vertical_torque_temp1_R, joint_vertical_torque_temp2_R; + fp32 joint_vertical_torque_temp1_L, joint_vertical_torque_temp2_L; + +} torque_info_t; + +typedef struct +{ + fp32 J1_L,J2_L; + fp32 J3_L,J4_L; + fp32 J1_R,J2_R; + fp32 J3_R,J4_R; + fp32 invJ1_L,invJ2_L; + fp32 invJ3_L,invJ4_L; + fp32 invJ1_R,invJ2_R; + fp32 invJ3_R,invJ4_R; + +} mapping_info_t; + +typedef struct +{ + const HT_motor_measure_t *motor_measure; + chassis_motor_mode_e motor_mode, last_motor_mode; + + bool_t offline_flag; + + fp32 position; + fp32 init_position; + fp32 position_offset; + + fp32 velocity; + fp32 torque_out, torque_get; + fp32 max_torque, min_torque; +} joint_motor_t; + +typedef struct +{ + motor_measure_t motor_measure; + chassis_motor_mode_e motor_mode, last_motor_mode; + + bool_t offline_flag; + + fp32 distance, distance_offset, last_position, position, turns; + fp32 speed; + fp32 torque_out, torque_get; + +} foot_motor_t; + +typedef struct +{ + bool_t init_flag; + suspend_flag_e suspend_flag_L, last_suspend_flag_L; + suspend_flag_e suspend_flag_R, last_suspend_flag_R; + bool_t Ignore_Off_Ground; + bool_t abnormal_flag; + bool_t static_flag, last_static_flag; + bool_t moving_flag, last_moving_flag; + bool_t rotation_flag; + bool_t controlling_flag; + bool_t set_pos_after_moving; + bool_t overpower_warning_flag; + bool_t last_overpower_warning_flag; + bool_t stablize_high_flag; + bool_t last_stablize_high_flag; +} flag_info_t; + +typedef struct +{ + pid_type_def buffer_control_pid; + pid_type_def cap_pid; + pid_type_def scale_down_pid; +} pid_info_t; + +typedef struct +{ + mode_t mode; + chassis_posture_info_t chassis_posture_info; + torque_info_t torque_info; + mapping_info_t mapping_info; + flag_info_t flag_info; + pid_info_t pid_info; + const Gimbal_ctrl_t *chassis_rc_ctrl; + + joint_motor_t joint_motor_1, joint_motor_2, joint_motor_3, joint_motor_4; + foot_motor_t foot_motor_L, foot_motor_R; + +}chassis_control_t; + +enum Chassis_Mode +{ + Chassis_No_Force = 0, + Follow_Gimbal, + Follow_Gimbal_90Deg, + No_Follow, + Rotate, +// TK_MODE, +}; + +extern enum Chassis_Mode chassis_mode; +extern chassis_control_t chassis_control; +extern fp32 roll_PD[2]; + +#endif diff --git a/utils/香港大学轮腿电控及建模开源-3/HerKules_VOCAL_SJ_LQR_v4_with_data.m b/utils/香港大学轮腿电控及建模开源-3/HerKules_VOCAL_SJ_LQR_v4_with_data.m new file mode 100644 index 0000000..e762696 --- /dev/null +++ b/utils/香港大学轮腿电控及建模开源-3/HerKules_VOCAL_SJ_LQR_v4_with_data.m @@ -0,0 +1,321 @@ +% v1：这份LQR程序是参考我之前写的哈工程LQR程序以及小周写的AB矩阵求解器优化后写出来的，感谢周神（2024/05/07） +% v2：添加了可以专门调试定腿长的功能（2024/05/08） +% v3：优化部分注释，添加单位说明（2024/05/15） +% v4: 优化了输出，输出矩阵K的系数可以真正的复制到C里（2024/05/16） + +% 以下所有变量含义参考2023上交轮腿电控开源（https://bbs.robomaster.com/forum.php?mod=viewthread&tid=22756）所使用符号含义 + +%%%%%%%%%%%%%%%%%%%%%%%%%Step 0：重置程序，定义变量%%%%%%%%%%%%%%%%%%%%%%%%% +tic +clear all +clc + +% 定义机器人机体参数 +syms R_w % 驱动轮半径 +syms R_l % 驱动轮轮距/2 +syms l_l l_r % 左右腿长 +syms l_wl l_wr % 驱动轮质心到左右腿部质心距离 +syms l_bl l_br % 机体质心到左右腿部质心距离 +syms l_c % 机体质心到腿部关节中心点距离 +syms m_w m_l m_b % 驱动轮质量 腿部质量 机体质量 +syms I_w % 驱动轮转动惯量 (自然坐标系法向) +syms I_ll I_lr % 驱动轮左右腿部转动惯量 (自然坐标系法向，实际上会变化) +syms I_b % 机体转动惯量 (自然坐标系法向) +syms I_z % 机器人z轴转动惯量 (简化为常量) + +% 定义其他独立变量并补充其导数 +syms theta_wl theta_wr % 左右驱动轮转角 +syms dtheta_wl dtheta_wr +syms ddtheta_wl ddtheta_wr ddtheta_ll ddtheta_lr ddtheta_b + +% 定义状态向量 +syms s ds phi dphi theta_ll dtheta_ll theta_lr dtheta_lr theta_b dtheta_b + +% 定义控制向量 +syms T_wl T_wr T_bl T_br + +% 输入物理参数：重力加速度 +syms g + + + +%%%%%%%%%%%%%%%%%%%%%%%Step 1：解方程，求控制矩阵A，B%%%%%%%%%%%%%%%%%%%%%%% + +% 通过原文方程组(3.11)-(3.15)，求出ddtheta_wl,ddtheta_wr,ddtheta_ll,ddtheta_lr,ddtheta_b表达式 +eqn1 = (I_w*l_l/R_w+m_w*R_w*l_l+m_l*R_w*l_bl)*ddtheta_wl+(m_l*l_wl*l_bl-I_ll)*ddtheta_ll+(m_l*l_wl+m_b*l_l/2)*g*theta_ll+T_bl-T_wl*(1+l_l/R_w)==0; +eqn2 = (I_w*l_r/R_w+m_w*R_w*l_r+m_l*R_w*l_br)*ddtheta_wr+(m_l*l_wr*l_br-I_lr)*ddtheta_lr+(m_l*l_wr+m_b*l_r/2)*g*theta_lr+T_br-T_wr*(1+l_r/R_w)==0; +eqn3 = -(m_w*R_w*R_w+I_w+m_l*R_w*R_w+m_b*R_w*R_w/2)*ddtheta_wl-(m_w*R_w*R_w+I_w+m_l*R_w*R_w+m_b*R_w*R_w/2)*ddtheta_wr-(m_l*R_w*l_wl+m_b*R_w*l_l/2)*ddtheta_ll-(m_l*R_w*l_wr+m_b*R_w*l_r/2)*ddtheta_lr+T_wl+T_wr==0; +eqn4 = (m_w*R_w*l_c+I_w*l_c/R_w+m_l*R_w*l_c)*ddtheta_wl+(m_w*R_w*l_c+I_w*l_c/R_w+m_l*R_w*l_c)*ddtheta_wr+m_l*l_wl*l_c*ddtheta_ll+m_l*l_wr*l_c*ddtheta_lr-I_b*ddtheta_b+m_b*g*l_c*theta_b-(T_wl+T_wr)*l_c/R_w-(T_bl+T_br)==0; +eqn5 = ((I_z*R_w)/(2*R_l)+I_w*R_l/R_w)*ddtheta_wl-((I_z*R_w)/(2*R_l)+I_w*R_l/R_w)*ddtheta_wr+(I_z*l_l)/(2*R_l)*ddtheta_ll-(I_z*l_r)/(2*R_l)*ddtheta_lr-T_wl*R_l/R_w+T_wr*R_l/R_w==0; +[ddtheta_wl,ddtheta_wr,ddtheta_ll,ddtheta_lr,ddtheta_b] = solve(eqn1,eqn2,eqn3,eqn4,eqn5,ddtheta_wl,ddtheta_wr,ddtheta_ll,ddtheta_lr,ddtheta_b); + + +% 通过计算雅可比矩阵的方法得出控制矩阵A，B所需要的全部偏导数 +J_A = jacobian([ddtheta_wl,ddtheta_wr,ddtheta_ll,ddtheta_lr,ddtheta_b],[theta_ll,theta_lr,theta_b]); +J_B = jacobian([ddtheta_wl,ddtheta_wr,ddtheta_ll,ddtheta_lr,ddtheta_b],[T_wl,T_wr,T_bl,T_br]); + +% 定义矩阵A，B，将指定位置的数值根据上述偏导数计算出来并填入 +A = sym('A',[10 10]); +B = sym('B',[10 4]); + +% 填入A数据：a25,a27,a29,a45,a47,a49,a65,a67,a69,a85,a87,a89,a105,a107,a109 +for p = 5:2:9 + A_index = (p - 3)/2; + A(2,p) = R_w*(J_A(1,A_index) + J_A(2,A_index))/2; + A(4,p) = (R_w*(- J_A(1,A_index) + J_A(2,A_index)))/(2*R_l) - (l_l*J_A(3,A_index))/(2*R_l) + (l_r*J_A(4,A_index))/(2*R_l); + for q = 6:2:10 + A(q,p) = J_A(q/2,A_index); + end +end + +% A的以下数值为1：a12,a34,a56,a78,a910，其余数值为0 +for r = 1:10 + if rem(r,2) == 0 + A(r,1) = 0; A(r,2) = 0; A(r,3) = 0; A(r,4) = 0; A(r,6) = 0; A(r,8) = 0; A(r,10) = 0; + else + A(r,:) = zeros(1,10); + A(r,r+1) = 1; + end +end + +% 填入B数据：b21,b22,b23,b24,b41,b42,b43,b44,b61,b62,b63,b64,b81,b82,b83,b84,b101,b102,b103,b104, +for h = 1:4 + B(2,h) = R_w*(J_B(1,h) + J_B(2,h))/2; + B(4,h) = (R_w*(- J_B(1,h) + J_B(2,h)))/(2*R_l) - (l_l*J_B(3,h))/(2*R_l) + (l_r*J_B(4,h))/(2*R_l); + for f = 6:2:10 + B(f,h) = J_B(f/2,h); + end +end + +% B的其余数值为0 +for e = 1:2:9 + B(e,:) = zeros(1,4); +end + + + +%%%%%%%%%%%%%%%%%%%%%Step 2：输入参数（可以修改的部分）%%%%%%%%%%%%%%%%%%%%% + +% 物理参数赋值（唯一此处不可改变！），后面的数据通过增加后缀_ac区分模型符号和实际数据 +g_ac = 9.81; + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% 此处可以输入机器人机体基本参数 % +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +%%%%%%%%%%%%%%%%%%%%%%%%%%%机器人机体与轮部参数%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +R_w_ac = 0.9; % 驱动轮半径 （单位：m） +R_l_ac = 0.25; % 两个驱动轮之间距离/2 （单位：m） +l_c_ac = 0.037; % 机体质心到腿部关节中心点距离 （单位：m） +m_w_ac = 0.8; m_l_ac = 1.6183599; m_b_ac = 11.542; % 驱动轮质量 腿部质量 机体质量 （单位：kg） +I_w_ac = (3510000)*10^(-7); % 驱动轮转动惯量 （单位：kg m^2） +I_b_ac = 0.260; % 机体转动惯量(自然坐标系法向) （单位：kg m^2） +I_z_ac = 0.226; % 机器人z轴转动惯量 （单位：kg m^2） + +%%%%%%%%%%%%%%%%%%%%%%机器人腿部参数（定腿长调试用）%%%%%%%%%%%%%%%%%%%%%%%% + +% 如果需要使用此部分，请去掉120-127行以及215-218行注释，然后将224行之后的所有代码注释掉 +% 或者点击左侧数字"224"让程序只运行行之前的内容并停止 + +l_l_ac = 0.18; % 左腿摆杆长度 （左腿对应数据） （单位：m） +l_wl_ac = 0.05; % 左驱动轮质心到左腿摆杆质心距离 （单位：m） +l_bl_ac = 0.13; % 机体转轴到左腿摆杆质心距离 （单位：m） +I_ll_ac = 0.02054500; % 左腿摆杆转动惯量 （单位：kg m^2） +l_r_ac = 0.18; % 右腿摆杆长度 （右腿对应数据） （单位：m） +l_wr_ac = 0.05; % 右驱动轮质心到右腿摆杆质心距离 （单位：m） +l_br_ac = 0.13; % 机体转轴到右腿摆杆质心距离 （单位：m） +I_lr_ac = 0.02054500; % 右腿摆杆转动惯量 （单位：kg m^2） + +% 机体转轴定义参考哈工程开源（https://zhuanlan.zhihu.com/p/563048952），是左右 +% 两侧两个关节电机之间的中间点相连所形成的轴 +% （如果目的是小板凳，考虑使左右腿相关数据一致） + +%%%%%%%%%%%%%%%%%%%%%%%%%%%机器人腿部参数数据集%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +% 根据不同腿长长度，先针对左腿测量出对应的l_wl，l_bl，和I_ll +% 通过以下方式记录数据: 矩阵分4列， +% 第一列为左腿腿长范围区间中所有小数点精度0.01的长度，例如：0.09，0.18，单位：m +% 第二列为l_wl，单位：m +% 第三列为l_bl，单位：m +% 第四列为I_ll，单位：kg m^2 +% （注意单位别搞错！） +% 行数根据L_0范围区间（，单位cm时）的整数数量进行调整 + +Leg_data_l = [0.11, 0.0480, 0.0620, 0.01819599; + 0.12, 0.0470, 0.0730, 0.01862845; + 0.13, 0.0476, 0.0824, 0.01898641; + 0.14, 0.0480, 0.0920, 0.01931342; + 0.15, 0.0490, 0.1010, 0.01962521; + 0.16, 0.0500, 0.1100, 0.01993092; + 0.17, 0.0510, 0.1190, 0.02023626; + 0.18, 0.0525, 0.1275, 0.02054500; + 0.19, 0.0539, 0.1361, 0.02085969; + 0.20, 0.0554, 0.1446, 0.02118212; + 0.21, 0.0570, 0.1530, 0.02151357; + 0.22, 0.0586, 0.1614, 0.02185496; + 0.23, 0.0600, 0.1700, 0.02220695; + 0.24, 0.0621, 0.1779, 0.02256999; + 0.25, 0.0639, 0.1861, 0.02294442; + 0.26, 0.0657, 0.1943, 0.02333041; + 0.27, 0.0676, 0.2024, 0.02372806; + 0.28, 0.0700, 0.2100, 0.02413735; + 0.29, 0.0713, 0.2187, 0.02455817; + 0.30, 0.0733, 0.2267, 0.02499030]; +% 以上数据应通过实际测量或sw图纸获得 + +% 由于左右腿部数据通常完全相同，我们通过复制的方式直接定义右腿的全部数据集 +% 矩阵分4列，第一列为右腿腿长范围区间中（，单位cm时）的整数腿长l_r*0.01，第二列为l_wr，第三列为l_br，第四列为I_lr） +Leg_data_r = Leg_data_l; + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% 此处可以输入QR矩阵 % +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +% 矩阵Q中，以下列分别对应： +% s ds phi dphi theta_ll dtheta_ll theta_lr dtheta_lr theta_b dtheta_b +lqr_Q = [1, 0, 0, 0, 0, 0, 0, 0, 0, 0; + 0, 2, 0, 0, 0, 0, 0, 0, 0, 0; + 0, 0, 12000, 0, 0, 0, 0, 0, 0, 0; + 0, 0, 0, 200, 0, 0, 0, 0, 0, 0; + 0, 0, 0, 0, 1000, 0, 0, 0, 0, 0; + 0, 0, 0, 0, 0, 1, 0, 0, 0, 0; + 0, 0, 0, 0, 0, 0, 1000, 0, 0, 0; + 0, 0, 0, 0, 0, 0, 0, 1, 0, 0; + 0, 0, 0, 0, 0, 0, 0, 0, 20000, 0; + 0, 0, 0, 0, 0, 0, 0, 0, 0, 1]; +% 其中： +% s : 自然坐标系下机器人水平方向移动距离，单位：m，ds为其导数 +% phi ：机器人水平方向移动时yaw偏航角度，dphi为其导数 +% theta_ll：左腿摆杆与竖直方向（自然坐标系z轴）夹角，dtheta_ll为其导数 +% theta_lr：右腿摆杆与竖直方向（自然坐标系z轴）夹角，dtheta_lr为其导数 +% theta_b ：机体与自然坐标系水平夹角，dtheta_b为其导数 + +% 矩阵中，以下列分别对应： +% T_wl T_wr T_bl T_br +lqr_R = [0.25, 0, 0, 0; + 0, 0.25, 0, 0; + 0, 0, 1.5, 0; + 0, 0, 0, 1.5]; +% 其中： +% T_wl: 左侧驱动轮输出力矩 +% T_wr：右侧驱动轮输出力矩 +% T_bl：左侧髋关节输出力矩 +% T_br：右腿髋关节输出力矩 +% 单位皆为Nm + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + + +%%%%%%%%%%%%%%%%%%%%%Step 2.5：求解矩阵（定腿长调试用）%%%%%%%%%%%%%%%%%%%%% + +% 如果需要使用此部分，请去掉120-127行以及215-218行注释，然后将224行之后的所有代码注释掉， +% 或者点击左侧数字"224"让程序只运行行之前的内容并停止 +K = get_K_from_LQR(R_w,R_l,l_l,l_r,l_wl,l_wr,l_bl,l_br,l_c,m_w,m_l,m_b,I_w,I_ll,I_lr,I_b,I_z,g, ... + R_w_ac,R_l_ac,l_l_ac,l_r_ac,l_wl_ac,l_wr_ac,l_bl_ac,l_br_ac, ... + l_c_ac,m_w_ac,m_l_ac,m_b_ac,I_w_ac,I_ll_ac,I_lr_ac,I_b_ac,I_z_ac,g_ac, ... + A,B,lqr_Q,lqr_R) +K = sprintf([strjoin(repmat({'%.5g'},1,size(K,2)),', ') '\n'], K.') + + +%%%%%%%%%%%%%%%%%%%%%%%%%%Step 3：拟合控制律函数%%%%%%%%%%%%%%%%%%%%%%%%%% + +sample_size = size(Leg_data_l,1)^2; % 单个K_ij拟合所需要的样本数 + +length = size(Leg_data_l,1); % 测量腿部数据集的行数 + +% 定义所有K_ij依据l_l,l_r变化的表格，每一个表格有3列，第一列是l_l，第二列 +% 是l_r，第三列是对应的K_ij的数值 +K_sample = zeros(sample_size,3,40); % 40是因为增益矩阵K应该是4行10列。 + +for i = 1:length + for j = 1:length + index = (i - 1)*length + j; + l_l_ac = Leg_data_l(i,1); % 提取左腿对应的数据 + l_wl_ac = Leg_data_l(i,2); + l_bl_ac = Leg_data_l(i,3); + I_ll_ac = Leg_data_l(i,4); + l_r_ac = Leg_data_r(j,1); % 提取右腿对应的数据 + l_wr_ac = Leg_data_r(j,2); + l_br_ac = Leg_data_r(j,3); + I_lr_ac = Leg_data_r(j,4); + for k = 1:40 + K_sample(index,1,k) = l_l_ac; + K_sample(index,2,k) = l_r_ac; + end + K = get_K_from_LQR(R_w,R_l,l_l,l_r,l_wl,l_wr,l_bl,l_br,l_c,m_w,m_l,m_b,I_w,I_ll,I_lr,I_b,I_z,g, ... + R_w_ac,R_l_ac,l_l_ac,l_r_ac,l_wl_ac,l_wr_ac,l_bl_ac,l_br_ac, ... + l_c_ac,m_w_ac,m_l_ac,m_b_ac,I_w_ac,I_ll_ac,I_lr_ac,I_b_ac,I_z_ac,g_ac, ... + A,B,lqr_Q,lqr_R); + % 根据指定的l_l,l_r输入对应的K_ij的数值 + for l = 1:4 + for m = 1:10 + K_sample(index,3,(l - 1)*10 + m) = K(l,m); + end + end + end +end + +% 创建收集全部K_ij的多项式拟合的全部系数的集合 +K_Fit_Coefficients = zeros(40,6); +for n = 1:40 + K_Surface_Fit = fit([K_sample(:,1,n),K_sample(:,2,n)],K_sample(:,3,n),'poly22'); + K_Fit_Coefficients(n,:) = coeffvalues(K_Surface_Fit); % 拟合并提取出拟合的系数结果 +end +Polynomial_expression = formula(K_Surface_Fit) + +% 最终返回的结果K_Fit_Coefficients是一个40行6列矩阵，每一行分别对应一个K_ij的多项式拟合的全部系数 +% 每一行和K_ij的对应关系如下： +% - 第1行对应K_1,1 +% - 第14行对应K_2,4 +% - 第22行对应K_3,2 +% - 第37行对应K_4,7 +% ... 其他行对应关系类似 +% 拟合出的函数表达式为 p(x,y) = p00 + p10*x + p01*y + p20*x^2 + p11*x*y + p02*y^2 +% 其中x对应左腿腿长l_l，y对应右腿腿长l_r +% K_Fit_Coefficients每一列分别对应全部K_ij的多项式拟合的单个系数 +% 每一列和系数pij的对应关系如下： +% - 第1列对应p00 +% - 第2列对应p10 +% - 第3列对应p01 +% - 第4列对应p20 +% - 第5列对应p11 +% - 第6列对应p02 +K_Fit_Coefficients = sprintf([strjoin(repmat({'%.5g'},1,size(K_Fit_Coefficients,2)),', ') '\n'], K_Fit_Coefficients.') + +% 正确食用方法： +% 1.在C代码中写出控制律K矩阵的全部多项式，其中每一个多项式的表达式为： +% p(l_l,l_r) = p00 + p10*l_l + p01*l_r + p20*l_l^2 + p11*l_l*l_r + p02*l_r^2 +% 2.并填入对应系数即可 + +toc + +%%%%%%%%%%%%%%%%%%%%%%%%%以下信息仅供参考，可忽略%%%%%%%%%%%%%%%%%%%%%%%%%% + +% 如有需要可以把所有K_ij画出图来参考，可以去掉以下注释 +% 此版本只能同时查看其中一个K_ij，同时查看多个的功能下次更新 +% （前面的蛆，以后再来探索吧（bushi + + + +%%%%%%%%%%%%%%%%%%%%%%%%%%得出控制矩阵K使用的函数%%%%%%%%%%%%%%%%%%%%%%%%%% + +function K = get_K_from_LQR(R_w,R_l,l_l,l_r,l_wl,l_wr,l_bl,l_br,l_c,m_w,m_l,m_b,I_w,I_ll,I_lr,I_b,I_z,g, ... + R_w_ac,R_l_ac,l_l_ac,l_r_ac,l_wl_ac,l_wr_ac,l_bl_ac,l_br_ac, ... + l_c_ac,m_w_ac,m_l_ac,m_b_ac,I_w_ac,I_ll_ac,I_lr_ac,I_b_ac,I_z_ac,g_ac, ... + A,B,lqr_Q,lqr_R) + % 基于机体以及物理参数，获得控制矩阵A，B的全部数值 + A_ac = subs(A,[R_w R_l l_l l_r l_wl l_wr l_bl l_br l_c m_w m_l m_b I_w I_ll I_lr I_b I_z g], ... + [R_w_ac R_l_ac l_l_ac l_r_ac l_wl_ac l_wr_ac l_bl_ac l_br_ac l_c_ac ... + m_w_ac m_l_ac m_b_ac I_w_ac I_ll_ac I_lr_ac I_b_ac I_z_ac g_ac]); + B_ac = subs(B,[R_w R_l l_l l_r l_wl l_wr l_bl l_br l_c m_w m_l m_b I_w I_ll I_lr I_b I_z g], ... + [R_w_ac R_l_ac l_l_ac l_r_ac l_wl_ac l_wr_ac l_bl_ac l_br_ac l_c_ac ... + m_w_ac m_l_ac m_b_ac I_w_ac I_ll_ac I_lr_ac I_b_ac I_z_ac g_ac]); + + % 根据以上信息和提供的矩阵Q和R求解Riccati方程，获得增益矩阵K + % P为Riccati方程的解，矩阵L可以无视 + [P,K,L_k] = icare(A_ac,B_ac,lqr_Q,lqr_R,[],[],[]); +end + diff --git a/utils/香港大学轮腿电控及建模开源-3/some_functions.c b/utils/香港大学轮腿电控及建模开源-3/some_functions.c new file mode 100644 index 0000000..5dbbd5b --- /dev/null +++ b/utils/香港大学轮腿电控及建模开源-3/some_functions.c @@ -0,0 +1,304 @@ +// Front +// | 1 4 | +// (1)Left| |Right(2) +// | 2 3 | + +#define HT_SLAVE_ID1 0x01 +#define HT_SLAVE_ID2 0x02 +#define HT_SLAVE_ID3 0x03 +#define HT_SLAVE_ID4 0x04 + +#define CAN_BM_M1_ID 0x97 +#define CAN_BM_M2_ID 0x98 +#define BM_CAN hcan2 + +#define get_HT_motor_measure(ptr, data) \ +{ \ + (ptr)->last_ecd = (ptr)->ecd; \ + (ptr)->ecd = uint_to_float((uint16_t)((data)[1] << 8 | (data)[2] ),P_MIN, P_MAX, 16); \ + (ptr)->speed_rpm = uint_to_float((uint16_t)(data[3]<<4)|(data[4]>>4), V_MIN, V_MAX, 12);\ + (ptr)->real_torque = uint_to_float((uint16_t)(((data[4] & 0x0f) << 8) | (data)[5]), -18, +18, 12); \ +} + +#define get_BM_motor_measure(ptr, data) \ +{ \ + (ptr)->last_ecd = (ptr)->ecd; \ + (ptr)->ecd = (uint16_t)((data)[4] << 8 | (data)[5]); \ + (ptr)->speed_rpm = (int16_t)((data)[0] << 8 | (data)[1]); \ + (ptr)->given_current = (int16_t)((data)[2] << 8 | (data)[3]); \ + (ptr)->error = (data)[6]; \ + (ptr)->mode = (data)[7]; \ +} + +void CAN_BM_ENABLE_CMD(void) +{ + CAN_TxHeaderTypeDef bm_tx_measure; + uint8_t bm_can_send_data[8]; + + uint32_t send_mail_box; + bm_tx_measure.StdId = 0x105; // 120 + bm_tx_measure.IDE = CAN_ID_STD; + bm_tx_measure.RTR = CAN_RTR_DATA; + bm_tx_measure.DLC = 0x08; + bm_can_send_data[0] = 0x0A; + bm_can_send_data[1] = 0x0A; + bm_can_send_data[2] = 0x00; + bm_can_send_data[3] = 0x00; + bm_can_send_data[4] = 0x00; + bm_can_send_data[5] = 0x00; + bm_can_send_data[6] = 0x00; + bm_can_send_data[7] = 0x00; + + HAL_CAN_AddTxMessage(&BM_CAN, &bm_tx_measure, bm_can_send_data, &send_mail_box); +} + +void CAN_BM_MODE_SET_CMD(void) +{ + CAN_TxHeaderTypeDef bm_tx_measure; + uint8_t bm_can_send_data[8]; + + uint32_t send_mail_box; + bm_tx_measure.StdId = 0x106; // 120 + bm_tx_measure.IDE = CAN_ID_STD; + bm_tx_measure.RTR = CAN_RTR_DATA; + bm_tx_measure.DLC = 0x08; + bm_can_send_data[0] = 0x01; + bm_can_send_data[1] = 0x01; + bm_can_send_data[2] = 0x00; + bm_can_send_data[3] = 0x00; + bm_can_send_data[4] = 0x00; + bm_can_send_data[5] = 0x00; + bm_can_send_data[6] = 0x00; + bm_can_send_data[7] = 0x00; + + HAL_CAN_AddTxMessage(&BM_CAN, &bm_tx_measure, bm_can_send_data, &send_mail_box); +} + +void CAN_BM_CONTROL_CMD( int16_t motor1, int16_t motor2 ) +{ + CAN_TxHeaderTypeDef bm_tx_measure; + uint8_t bm_can_send_data[8]; + + uint32_t send_mail_box; + bm_tx_measure.StdId = 0x32; // 120 + bm_tx_measure.IDE = CAN_ID_STD; + bm_tx_measure.RTR = CAN_RTR_DATA; + bm_tx_measure.DLC = 0x08; + bm_can_send_data[0] = motor1 >> 8; + bm_can_send_data[1] = motor1; + bm_can_send_data[2] = motor2 >> 8; + bm_can_send_data[3] = motor2; + bm_can_send_data[4] = 0x00; + bm_can_send_data[5] = 0x00; + bm_can_send_data[6] = 0x00; + bm_can_send_data[7] = 0x00; + + HAL_CAN_AddTxMessage(&BM_CAN, &bm_tx_measure, bm_can_send_data, &send_mail_box); +} + +#define LIMIT_MIN_MAX(x,min,max) (x) = (((x)<=(min))?(min):(((x)>=(max))?(max):(x))) +#define P_MIN -95.5f// Radians +#define P_MAX 95.5f +#define V_MIN -45.0f// Rad/s +#define V_MAX 45.0f +#define KP_MIN 0.0f// N-m/rad +#define KP_MAX 500.0f +#define KD_MIN 0.0f// N-m/rad/s +#define KD_MAX 5.0f +#define T_MIN -18.0f +#define T_MAX 18.0f + +void CAN_HT_CMD( uint8_t id, fp32 f_t ) +{ + uint32_t canTxMailbox = CAN_TX_MAILBOX0; + + fp32 f_p = 0.0f, f_v = 0.0f, f_kp = 0.0f, f_kd = 0.0f; + uint16_t p, v, kp, kd, t; + uint8_t buf[8]; + LIMIT_MIN_MAX(f_p, P_MIN, P_MAX); + LIMIT_MIN_MAX(f_v, V_MIN, V_MAX); + LIMIT_MIN_MAX(f_kp, KP_MIN, KP_MAX); + LIMIT_MIN_MAX(f_kd, KD_MIN, KD_MAX); + LIMIT_MIN_MAX(f_t, T_MIN, T_MAX); + + p = float_to_uint(f_p, P_MIN, P_MAX, 16); + v = float_to_uint(f_v, V_MIN, V_MAX, 12); + kp = float_to_uint(f_kp, KP_MIN, KP_MAX, 12); + kd = float_to_uint(f_kd, KD_MIN, KD_MAX, 12); + t = float_to_uint(f_t, T_MIN, T_MAX, 12); + + buf[0] = p>>8; + buf[1] = p&0xFF; + buf[2] = v>>4; + buf[3] = ((v&0xF)<<4)|(kp>>8); + buf[4] = kp&0xFF; + buf[5] = kd>>4; + buf[6] = ((kd&0xF)<<4)|(t>>8); + buf[7] = t&0xff; + + chassis_tx_message.StdId = id; + chassis_tx_message.IDE = CAN_ID_STD; + chassis_tx_message.RTR = CAN_RTR_DATA; + chassis_tx_message.DLC = 0x08; + + // while (HAL_CAN_GetTxMailboxesFreeLevel(&hcan1) == 0); + if ((hcan1.Instance->TSR & CAN_TSR_TME0) != RESET) + {canTxMailbox = CAN_TX_MAILBOX0;} + else if ((hcan1.Instance->TSR & CAN_TSR_TME1) != RESET) + {canTxMailbox = CAN_TX_MAILBOX1;} + else if ((hcan1.Instance->TSR & CAN_TSR_TME2) != RESET) + {canTxMailbox = CAN_TX_MAILBOX2;} + + if(HAL_CAN_AddTxMessage(&hcan1, &chassis_tx_message, buf, (uint32_t *)&canTxMailbox)==HAL_OK){}; +} +void CAN_CMD_HT_Enable(uint8_t id, uint8_t unterleib_motor_send_data[8] ) +{ + uint32_t canTxMailbox= CAN_TX_MAILBOX0; + + chassis_tx_message.StdId = id; + chassis_tx_message.IDE = CAN_ID_STD; + chassis_tx_message.RTR = CAN_RTR_DATA; + chassis_tx_message.DLC = 0x08; + + // while (HAL_CAN_GetTxMailboxesFreeLevel(&hcan1) == 0); + if ((hcan1.Instance->TSR & CAN_TSR_TME0) != RESET) + {canTxMailbox = CAN_TX_MAILBOX0;} + else if ((hcan1.Instance->TSR & CAN_TSR_TME1) != RESET) + {canTxMailbox = CAN_TX_MAILBOX1;} + else if ((hcan1.Instance->TSR & CAN_TSR_TME2) != RESET) + {canTxMailbox = CAN_TX_MAILBOX2;} + + if(HAL_CAN_AddTxMessage(&hcan1, &chassis_tx_message, unterleib_motor_send_data, (uint32_t *)&canTxMailbox)==HAL_OK){}; +} + +void Forward_kinematic_solution(chassis_control_t *feedback_update, + fp32 Q1,fp32 S1,fp32 Q4,fp32 S4, uint8_t ce) +{ + fp32 dL0=0,L0=0,Q0=0,S0=0; + fp32 xb,xd,yb,yd,Lbd,xc,yc; + fp32 A0,B0,C0,Q2,Q3,S2,S3; + fp32 vxb,vxd,vyb,vyd,vxc,vyc; + fp32 cos_Q1,cos_Q4,sin_Q1,sin_Q4; + fp32 sin_Q2,cos_Q2,sin_Q3,cos_Q3; + fp32 axb,ayb,axd,ayd,a2,axc; + /******************************/ + Q1 = PI + Q1; + cos_Q1 = arm_cos_f32(Q1); + sin_Q1 = arm_sin_f32(Q1); + cos_Q4 = arm_cos_f32(Q4); + sin_Q4 = arm_sin_f32(Q4); + xb = -L5/2.0f + L1*cos_Q1; + xd = L5/2.0f + L4*cos_Q4; + yb = L1*sin_Q1; + yd = L4*sin_Q4; + + Lbd=(xd-xb)*(xd-xb)+(yd-yb)*(yd-yb); + A0 = 2.0f*L2*(xd-xb); + B0 = 2.0f*L2*(yd-yb); + C0 = L2*L2+Lbd-L3*L3; + Q2 = 2.0f *atan_tl((B0+Sqrt(A0*A0 + B0*B0 -C0*C0))/(A0+C0)); + + xc = xb + arm_cos_f32(Q2)*L2; + yc = yb + arm_sin_f32(Q2)*L2; + + L0=Sqrt( xc*xc + yc*yc ); + Q0 = atan(yc/xc); + + vxb = -S1*L1*sin_Q1; + vyb = S1*L1*cos_Q1; + vxd = -S4*L4*sin_Q4; + vyd = S4*L4*cos_Q4; + Q3 = atan_tl((yc-yd)/(xc-xd)); + S2 = ((vxd-vxb)*arm_cos_f32(Q3) + (vyd-vyb)*arm_sin_f32(Q3))/(L2*arm_sin_f32(Q3-Q2)); + S3 = ((vxd-vxb)*arm_cos_f32(Q2) + (vyd-vyb)*arm_sin_f32(Q2))/(L2*arm_sin_f32(Q3-Q2)); + vxc = vxb - S2*L2*arm_sin_f32(Q2); + vyc = vyb + S2*L2*arm_cos_f32(Q2); + S0 = 3*(-arm_sin_f32(ABS(Q0))*vxc-arm_cos_f32(Q0)*vyc); + + if( Q0 < 0 ) + Q0 += PI; + /*******************************/ + if (ce) + { + feedback_update->chassis_posture_info .leg_length_L = L0; + feedback_update->chassis_posture_info .leg_angle_L = Q0; + // feedback_update->chassis_posture_info .leg_gyro_L = S0; + } + else + { + feedback_update->chassis_posture_info .leg_length_R = L0; + feedback_update->chassis_posture_info .leg_angle_R = Q0; + // feedback_update->chassis_posture_info .leg_gyro_R = S0; + } +} + +void Supportive_Force_Cal( chassis_control_t *Suspend_Detect, fp32 Q1, fp32 Q4, fp32 ce ) +{ + fp32 dL0=0,L0=0,Q0=0,S0=0; + fp32 xb,xd,yb,yd,Lbd,xc,yc; + fp32 A0,B0,C0,Q2,Q3,S2,S3; + fp32 vxb,vxd,vyb,vyd,vxc,vyc; + fp32 cos_Q1,cos_Q4,sin_Q1,sin_Q4; + fp32 sin_Q2,cos_Q2,sin_Q3,cos_Q3; + fp32 axb,ayb,axd,ayd,a2,axc; + /******************************/ + Q1 = PI + Q1; + cos_Q1 = arm_cos_f32(Q1); + sin_Q1 = arm_sin_f32(Q1); + cos_Q4 = arm_cos_f32(Q4); + sin_Q4 = arm_sin_f32(Q4); + xb = -L5/2.0f + L1*cos_Q1; + xd = L5/2.0f + L4*cos_Q4; + yb = L1*sin_Q1; + yd = L4*sin_Q4; + + Lbd=sqrt( (xd-xb)*(xd-xb)+(yd-yb)*(yd-yb) ); + A0 = 2.0f*L2*(xd-xb); + B0 = 2.0f*L2*(yd-yb); + C0 = L2*L2+Lbd*Lbd-L3*L3; + Q2 = 2.0f *atan_tl((B0+Sqrt(A0*A0 + B0*B0 -C0*C0))/(A0+C0)); + xc = xb + arm_cos_f32(Q2)*L2; + yc = yb + arm_sin_f32(Q2)*L2; + + L0 = Sqrt( xc*xc + yc*yc ); + Q0 = atan_tl(yc/xc); + if( Q0 < 0 ) + Q0 += PI; + + Q3 = atan_tl((yc-yd)/(xc-xd))+PI; + + + + fp32 J1 = ( L1 * arm_sin_f32(Q0 - Q3) * arm_sin_f32( Q1 - Q2 ) ) / (arm_sin_f32( Q3 - Q2 )); + fp32 J2 = ( L1 * arm_cos_f32(Q0 - Q3) * arm_sin_f32( Q1 - Q2 ) ) / (arm_sin_f32( Q3 - Q2 ) * L0); + fp32 J3 = ( L4 * arm_sin_f32(Q0 - Q2) * arm_sin_f32( Q3 - Q4 ) ) / (arm_sin_f32( Q3 - Q2 )); + fp32 J4 = ( L4 * arm_cos_f32(Q0 - Q2) * arm_sin_f32( Q3 - Q4 ) ) / (arm_sin_f32( Q3 - Q2 ) * L0); + + + fp32 dett = J1 * J4 - J2 * J3; + fp32 Inv_J1 = J4 / dett; + fp32 Inv_J2 = -J2 / dett; + fp32 Inv_J3 = -J3 / dett; + fp32 Inv_J4 = J1 / dett; + + ddebug = yc; + + if( ce == 1.0f ) + { + Suspend_Detect->chassis_posture_info.supportive_force_L = + Inv_J1 * Suspend_Detect->joint_motor_1.torque_get + + Inv_J2 * Suspend_Detect->joint_motor_2.torque_get; + // if( Suspend_Detect->chassis_posture_info.supportive_force_L < 0.0f ) + // Suspend_Detect->chassis_posture_info.supportive_force_L += 40.0f; + } + else + { + Suspend_Detect->chassis_posture_info.supportive_force_R = + Inv_J1 * Suspend_Detect->joint_motor_4.torque_get + + Inv_J2 * Suspend_Detect->joint_motor_3.torque_get; + Suspend_Detect->chassis_posture_info.supportive_force_R *= -1.0f; + // if( Suspend_Detect->chassis_posture_info.supportive_force_R < 0.0f ) + // Suspend_Detect->chassis_posture_info.supportive_force_R += 40.0f; + } + +} \ No newline at end of file diff --git a/utils/香港大学轮腿电控及建模开源-3/香港大学轮腿平衡步兵机械&电控解决方案概括.pdf b/utils/香港大学轮腿电控及建模开源-3/香港大学轮腿平衡步兵机械&电控解决方案概括.pdf new file mode 100644 index 0000000..d9966cb Binary files /dev/null and b/utils/香港大学轮腿电控及建模开源-3/香港大学轮腿平衡步兵机械&电控解决方案概括.pdf differ