加个音乐
This commit is contained in:
parent
ce85f0f66c
commit
12016916ef
@ -13,7 +13,8 @@
|
|||||||
#define MUSIC_DEFAULT_VOLUME 0.5f
|
#define MUSIC_DEFAULT_VOLUME 0.5f
|
||||||
#define MUSIC_A4_FREQ 440.0f // A4音符频率
|
#define MUSIC_A4_FREQ 440.0f // A4音符频率
|
||||||
|
|
||||||
|
/* USER MUSIC MENU BEGIN */
|
||||||
|
// RM音乐
|
||||||
const Tone_t RM[] = {
|
const Tone_t RM[] = {
|
||||||
{NOTE_B, 5, 200},
|
{NOTE_B, 5, 200},
|
||||||
{NOTE_G, 4, 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_B, 4, 125}, {NOTE_A, 4, 125}, {NOTE_CS, 4, 250}, {NOTE_E, 4, 250},
|
||||||
{NOTE_A, 4, 500}
|
{NOTE_A, 4, 500}
|
||||||
};
|
};
|
||||||
|
/* USER MUSIC MENU END */
|
||||||
|
|
||||||
static void BUZZER_Update(BUZZER_t *buzzer){
|
static void BUZZER_Update(BUZZER_t *buzzer){
|
||||||
buzzer->header.online = true;
|
buzzer->header.online = true;
|
||||||
|
|||||||
@ -1,3 +1,11 @@
|
|||||||
|
/**
|
||||||
|
* @file buzzer.h
|
||||||
|
* @brief 蜂鸣器设备驱动头文件
|
||||||
|
* @details 提供蜂鸣器音频播放功能，支持单音符播放和预设音乐播放
|
||||||
|
* @author Generated by STM32CubeMX
|
||||||
|
* @date 2025年10月23日
|
||||||
|
*/
|
||||||
|
|
||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
#ifdef __cplusplus
|
#ifdef __cplusplus
|
||||||
@ -5,10 +13,10 @@ extern "C" {
|
|||||||
#endif
|
#endif
|
||||||
|
|
||||||
/* Includes ----------------------------------------------------------------- */
|
/* Includes ----------------------------------------------------------------- */
|
||||||
#include "device.h"
|
#include "bsp/pwm.h" // PWM底层硬件抽象层
|
||||||
#include "bsp/pwm.h"
|
#include "device.h" // 设备通用头文件
|
||||||
#include <stddef.h>
|
#include <stddef.h> // 标准定义
|
||||||
#include <stdint.h>
|
#include <stdint.h> // 标准整型定义
|
||||||
|
|
||||||
/* USER INCLUDE BEGIN */
|
/* USER INCLUDE BEGIN */
|
||||||
|
|
||||||
@ -22,37 +30,54 @@ extern "C" {
|
|||||||
|
|
||||||
/* Exported types ----------------------------------------------------------- */
|
/* Exported types ----------------------------------------------------------- */
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief 音符枚举类型
|
||||||
|
* @details 定义标准十二平均律音符，用于音乐播放
|
||||||
|
*/
|
||||||
typedef enum {
|
typedef enum {
|
||||||
NOTE_C = 0,
|
NOTE_C = 0, ///< Do音符
|
||||||
NOTE_CS = 1, // C#
|
NOTE_CS = 1, ///< Do#音符 (升Do)
|
||||||
NOTE_D = 2,
|
NOTE_D = 2, ///< Re音符
|
||||||
NOTE_DS = 3, // D#
|
NOTE_DS = 3, ///< Re#音符 (升Re)
|
||||||
NOTE_E = 4,
|
NOTE_E = 4, ///< Mi音符
|
||||||
NOTE_F = 5,
|
NOTE_F = 5, ///< Fa音符
|
||||||
NOTE_FS = 6, // F#
|
NOTE_FS = 6, ///< Fa#音符 (升Fa)
|
||||||
NOTE_G = 7,
|
NOTE_G = 7, ///< Sol音符
|
||||||
NOTE_GS = 8, // G#
|
NOTE_GS = 8, ///< Sol#音符 (升Sol)
|
||||||
NOTE_A = 9,
|
NOTE_A = 9, ///< La音符
|
||||||
NOTE_AS = 10, // A#
|
NOTE_AS = 10, ///< La#音符 (升La)
|
||||||
NOTE_B = 11,
|
NOTE_B = 11, ///< Si音符
|
||||||
NOTE_REST = 255 // 休止符
|
NOTE_REST = 255 ///< 休止符 (无声音)
|
||||||
} NOTE_t;
|
} NOTE_t;
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief 音调结构体
|
||||||
|
* @details 定义一个完整的音调信息，包括音符、八度和持续时间
|
||||||
|
*/
|
||||||
typedef struct {
|
typedef struct {
|
||||||
NOTE_t note; // 音符名称
|
NOTE_t note; ///< 音符名称 (使用NOTE_t枚举)
|
||||||
uint8_t octave; // 八度
|
uint8_t octave; ///< 八度 (0-8，通常使用3-7)
|
||||||
uint16_t duration_ms; // 持续时间，单位毫秒
|
uint16_t duration_ms; ///< 持续时间，单位毫秒
|
||||||
} Tone_t;
|
} Tone_t;
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief 预设音乐枚举类型
|
||||||
|
* @details 定义可播放的预设音乐类型
|
||||||
|
*/
|
||||||
typedef enum {
|
typedef enum {
|
||||||
MUSIC_RM,
|
/* USER MUSIC MENU BEGIN */
|
||||||
MUSIC_NOKIA,
|
MUSIC_RM, ///< RM战队音乐
|
||||||
|
MUSIC_NOKIA, ///< 诺基亚经典铃声
|
||||||
|
/* USER MUSIC MENU END */
|
||||||
} MUSIC_t;
|
} MUSIC_t;
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief 蜂鸣器设备结构体
|
||||||
|
* @details 蜂鸣器设备的完整描述，包含设备头信息和PWM通道
|
||||||
|
*/
|
||||||
typedef struct {
|
typedef struct {
|
||||||
DEVICE_Header_t header;
|
DEVICE_Header_t header; ///< 设备通用头信息 (在线状态、时间戳等)
|
||||||
BSP_PWM_Channel_t channel;
|
BSP_PWM_Channel_t channel; ///< PWM输出通道
|
||||||
} BUZZER_t;
|
} BUZZER_t;
|
||||||
|
|
||||||
/* USER STRUCT BEGIN */
|
/* USER STRUCT BEGIN */
|
||||||
@ -61,14 +86,47 @@ typedef struct {
|
|||||||
|
|
||||||
/* Exported functions prototypes -------------------------------------------- */
|
/* 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);
|
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);
|
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);
|
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);
|
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);
|
int8_t BUZZER_PlayMusic(BUZZER_t *buzzer, MUSIC_t music);
|
||||||
|
|
||||||
/* USER FUNCTION BEGIN */
|
/* USER FUNCTION BEGIN */
|
||||||
|
|||||||
@ -254,19 +254,20 @@ Config_RobotParam_t robot_config = {
|
|||||||
}
|
}
|
||||||
},
|
},
|
||||||
|
|
||||||
|
|
||||||
.lqr_gains = {
|
.lqr_gains = {
|
||||||
.k11_coeff = { -3.147362972581901e+02f, 3.207074456637030e+02f, -1.299720619449884e+02f, -3.569397240304915e+00f }, // theta
|
.k11_coeff = { -2.299492214443364e+02f, 2.371048664025156e+02f, -1.091592921067477e+02f, -5.004256900558796e+00f }, // theta
|
||||||
.k12_coeff = { -1.586071559868343e+01f, 1.449431164705369e+01f, -1.121221716719879e+01f, -3.451792762764330e-01f }, // d_theta
|
.k12_coeff = { -5.160496889728024e+00f, 3.062519293243873e+00f, -9.339456933350174e+00f, -4.793280770575950e-01f }, // d_theta
|
||||||
.k13_coeff = { -5.618790901635980e+01f, 5.379369275290406e+01f, -1.745182841455727e+01f, -6.761399950784669e-01f }, // x
|
.k13_coeff = { -5.187383520564029e+01f, 4.899697460241380e+01f, -1.565478588426995e+01f, -2.004694192093775e+00f }, // x
|
||||||
.k14_coeff = { -7.173250530567481e+01f, 6.856776466550843e+01f, -2.301072504290219e+01f, -1.099155058815247e+00f }, // d_x
|
.k14_coeff = { -5.668796498905875e+01f, 5.377351750824888e+01f, -1.871718521370502e+01f, -2.691170008421150e+00f }, // d_x
|
||||||
.k15_coeff = { -9.613460546380958e+01f, 1.048862571784616e+02f, -4.025218968092935e+01f, 5.321247456640327e+00f }, // phi
|
.k15_coeff = { -1.449457451875909e+02f, 1.491544007677516e+02f, -5.400783326406858e+01f, 7.212250094701016e+00f }, // phi
|
||||||
.k16_coeff = { -7.052289754532913e+01f, 7.982089604487435e+01f, -3.307700050901695e+01f, 5.645737364902630e+00f }, // d_phi
|
.k16_coeff = { -9.623232912225313e+01f, 1.012699482439727e+02f, -3.862842162301360e+01f, 6.174165209610019e+00f }, // d_phi
|
||||||
.k21_coeff = { -1.057133693867297e+02f, 1.725695883025699e+02f, -9.737528883036143e+01f, 2.163196664625316e+01f }, // theta
|
.k21_coeff = { -1.197720213257983e+02f, 1.432558800683586e+02f, -6.332976803783767e+01f, 1.244403075562354e+01f }, // theta
|
||||||
.k22_coeff = { -2.315951596370687e+01f, 2.955906799408467e+01f, -1.354452600882797e+01f, 3.321564141904335e+00f }, // d_theta
|
.k22_coeff = { -1.641975451035667e+01f, 1.816373049685376e+01f, -6.825602612420409e+00f, 1.759808519224317e+00f }, // d_theta
|
||||||
.k23_coeff = { -8.976880402662755e+01f, 1.014166193118393e+02f, -4.187310761184219e+01f, 7.291792646751865e+00f }, // x
|
.k23_coeff = { -9.525505626411980e+01f, 9.986545276773818e+01f, -3.780741158172127e+01f, 6.150010121224194e+00f }, // x
|
||||||
.k24_coeff = { -1.341531876405906e+02f, 1.488885000296862e+02f, -6.019345895548503e+01f, 1.030125371802067e+01f }, // d_x
|
.k24_coeff = { -1.249520841685426e+02f, 1.297949868891588e+02f, -4.847618771898694e+01f, 7.842551207050604e+00f }, // d_x
|
||||||
.k25_coeff = { 1.384441101996959e+02f, -1.307641033174926e+02f, 4.125133391735927e+01f, 1.109615083184617e+01f }, // phi
|
.k25_coeff = { 1.098971996183004e+02f, -1.028972351169783e+02f, 3.234322461758097e+01f, 1.250515887186789e+01f }, // phi
|
||||||
.k26_coeff = { 1.826205273539437e+02f, -1.750288341017617e+02f, 5.676978575378812e+01f, 3.699479836742944e+00f }, // d_phi
|
.k26_coeff = { 1.122150774085550e+02f, -1.059683417765082e+02f, 3.379315768085248e+01f, 5.474171975795353e+00f }, // d_phi
|
||||||
},
|
},
|
||||||
.theta = 0.0f,
|
.theta = 0.0f,
|
||||||
.x = 0.0f,
|
.x = 0.0f,
|
||||||
|
|||||||
@ -29,15 +29,12 @@ void Task_music(void *argument) {
|
|||||||
|
|
||||||
/* USER CODE INIT BEGIN */
|
/* USER CODE INIT BEGIN */
|
||||||
BUZZER_Init(&buzzer, BSP_PWM_BUZZER);
|
BUZZER_Init(&buzzer, BSP_PWM_BUZZER);
|
||||||
|
BUZZER_PlayMusic(&buzzer, MUSIC_RM);
|
||||||
/* USER CODE INIT END */
|
/* USER CODE INIT END */
|
||||||
|
|
||||||
while (1) {
|
while (1) {
|
||||||
/* USER CODE BEGIN */
|
/* USER CODE BEGIN */
|
||||||
// BUZZER_PlayMusic(&buzzer, MUSIC_RM);
|
|
||||||
// BUZZER_Init(&buzzer, BSP_PWM_BUZZER);
|
|
||||||
|
|
||||||
BSP_TIME_Delay_ms(1000); // 等待音乐播放完成
|
|
||||||
/* USER CODE END */
|
/* USER CODE END */
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
@ -20,7 +20,7 @@ function K = get_k_length(leg_length)
|
|||||||
l1=-0.03; %机体质心距离转轴距离
|
l1=-0.03; %机体质心距离转轴距离
|
||||||
mw1=0.60; %驱动轮质量
|
mw1=0.60; %驱动轮质量
|
||||||
mp1=1.8; %杆质量
|
mp1=1.8; %杆质量
|
||||||
M1=10.0; %机体质量
|
M1=8.0; %机体质量
|
||||||
Iw1=mw1*R1^2; %驱动轮转动惯量
|
Iw1=mw1*R1^2; %驱动轮转动惯量
|
||||||
Ip1=mp1*((L1+LM1)^2+0.05^2)/12.0; %摆杆转动惯量
|
Ip1=mp1*((L1+LM1)^2+0.05^2)/12.0; %摆杆转动惯量
|
||||||
IM1=M1*(0.3^2+0.12^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=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);
|
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
|
Q=diag([160 160 2000 1500 20000 5000]);%theta d_theta x d_x phi d_phi%700 1 600 200 1000 1
|
||||||
R=[280 0;0 60]; %T Tp
|
R=[140 0;0 60]; %T Tp
|
||||||
|
|
||||||
K=lqr(A,B,Q,R);
|
K=lqr(A,B,Q,R);
|
||||||
|
|
||||||
|
|||||||
78
utils/k_calc/chassis_calc_heu.m
Normal file
78
utils/k_calc/chassis_calc_heu.m
Normal 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
1347
utils/香港大学轮腿电控及建模开源-3/Chassis_Task.c
Normal file
File diff suppressed because it is too large
Load Diff
299
utils/香港大学轮腿电控及建模开源-3/Chassis_Task.h
Normal file
299
utils/香港大学轮腿电控及建模开源-3/Chassis_Task.h
Normal 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
321
utils/香港大学轮腿电控及建模开源-3/HerKules_VOCAL_SJ_LQR_v4_with_data.m
Normal 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
304
utils/香港大学轮腿电控及建模开源-3/some_functions.c
Normal 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
BIN
utils/香港大学轮腿电控及建模开源-3/香港大学轮腿平衡步兵机械&电控解决方案概括.pdf
Normal file
Binary file not shown.
Loading…
Reference in New Issue
Block a user