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

加个音乐

Browse Source
This commit is contained in:
Robofish 2025-10-23 23:22:20 +08:00
parent ce85f0f66c
commit 12016916ef
11 changed files with 2453 additions and 46 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
User/device/buzzer.c
View File

@ -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;

108
User/device/buzzer.h
View File

@ -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 <stddef.h>
#include <stdint.h>
#include "bsp/pwm.h" // PWM底层硬件抽象层
#include "device.h" // 设备通用头文件
#include <stddef.h> // 标准定义
#include <stdint.h> // 标准整型定义
/* 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 */

25
User/module/config.c
View File

@ -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,

5
User/task/music.c
View File

@ -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 */
}

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

@ -20,7 +20,7 @@ function K = get_k_length(leg_length)
l1=-0.03; %机体质心距离转轴距离
mw1=0.60; %驱动轮质量
mp1=1.8; %杆质量
M1=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);

78
utils/k_calc/chassis_calc_heu.m Normal file
View File

@ -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)

1347
utils/香港大学轮腿电控及建模开源-3/Chassis_Task.c Normal file
View File

File diff suppressed because it is too large Load Diff

299
utils/香港大学轮腿电控及建模开源-3/Chassis_Task.h Normal file
View File

@ -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

321
utils/香港大学轮腿电控及建模开源-3/HerKules_VOCAL_SJ_LQR_v4_with_data.m Normal file
View File

@ -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

304
utils/香港大学轮腿电控及建模开源-3/some_functions.c Normal file
View File

@ -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;
}
}

BIN
utils/香港大学轮腿电控及建模开源-3/香港大学轮腿平衡步兵机械&电控解决方案概括.pdf Normal file
View File

Binary file not shown.