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

Compare commits

base: robofish:58f7ba86dbb6a0497aea7442461da28d29b37776
Branches Tags
robofish:main
robofish:MC_02
robofish:ai
..
compare: robofish:23ae0c3fa9d1f5568cdda73f74ece1690ddeb2bb
Branches Tags
robofish:MC_02
robofish:main
robofish:ai

No commits in common. "58f7ba86dbb6a0497aea7442461da28d29b37776" and "23ae0c3fa9d1f5568cdda73f74ece1690ddeb2bb" have entirely different histories.
58f7ba86db ... 23ae0c3fa9

88 changed files with 522 additions and 13917 deletions
Show Stats Expand all files Collapse all files

1358
Chassis_Task.c
View File

File diff suppressed because it is too large Load Diff

299
Chassis_Task.h
View File

@ -1,299 +0,0 @@
#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

12
Core/Src/freertos.c
View File

@ -27,7 +27,6 @@
/* Private includes ----------------------------------------------------------*/ /* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */ /* USER CODE BEGIN Includes */
#include "task/user_task.h"
/* USER CODE END Includes */ /* USER CODE END Includes */
/* Private typedef -----------------------------------------------------------*/ /* Private typedef -----------------------------------------------------------*/
@ -126,8 +125,7 @@ void MX_FREERTOS_Init(void) {
/* USER CODE BEGIN RTOS_THREADS */ /* USER CODE BEGIN RTOS_THREADS */
/* add threads, ... */ /* add threads, ... */
osThreadNew(Task_Init, NULL, &attr_init); // 创建初始化任务 /* USER CODE END RTOS_THREADS */
/* USER CODE END RTOS_THREADS */
/* USER CODE BEGIN RTOS_EVENTS */ /* USER CODE BEGIN RTOS_EVENTS */
/* add events, ... */ /* add events, ... */
@ -147,8 +145,12 @@ void StartDefaultTask(void *argument)
/* init code for USB_DEVICE */ /* init code for USB_DEVICE */
MX_USB_DEVICE_Init(); MX_USB_DEVICE_Init();
/* USER CODE BEGIN StartDefaultTask */ /* USER CODE BEGIN StartDefaultTask */
osThreadTerminate(osThreadGetId()); /* Infinite loop */
/* USER CODE END StartDefaultTask */ for(;;)
{
osDelay(1);
}
/* USER CODE END StartDefaultTask */
} }
/* Private application code --------------------------------------------------*/ /* Private application code --------------------------------------------------*/

1026
Core/Src/stm32f4xx_it.c
View File

File diff suppressed because it is too large Load Diff

321
HerKules_VOCAL_SJ_LQR_v4_with_data.m
View File

@ -1,321 +0,0 @@
% 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

67
LQR_修正建议.md
View File

@ -1,67 +0,0 @@
## LQR控制器修正建议
### 问题总结：
1. LQR增益计算方式不一致（3次多项式 vs 2次多项式）
2. 状态向量维度错误（12维 vs 10维）
3. 控制律映射不正确
4. 状态定义与MATLAB模型不匹配
### 修正建议：
#### 1. 修正LQR_K_calc函数
应该使用2次多项式而不是3次：
```cpp
float VMC::LQR_K_calc(float *coe, float l_l, float l_r) {
// 使用MATLAB中定义的2次多项式
// 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
return coe[0] + coe[1]*l_l + coe[2]*l_r + coe[3]*l_l*l_l + coe[4]*l_l*l_r + coe[5]*l_r*l_r;
}
```
#### 2. 修正增益矩阵维度
LQR增益矩阵应该是4×10，总共40个系数：
```cpp
// 为每条腿分配40个LQR系数，而不是12个
float LQR_K[40]; // 4×10矩阵，展开为一维数组
```
#### 3. 修正状态向量定义
确保状态向量与MATLAB模型一致：
```cpp
// 状态向量：[s, ds, phi, dphi, theta_ll, dtheta_ll, theta_lr, dtheta_lr, theta_b, dtheta_b]
float state_error[10] = {
move_argu_.xhat - move_argu_.target_x, // s误差
move_argu_.x_dot_hat - move_argu_.target_dot_x, // ds误差
this->yaw_ - 0.0f, // phi误差
this->gyro_.z - 0.0f, // dphi误差
leg_argu_[0].theta - 平衡角度, // theta_ll误差
leg_argu_[0].d_theta - 0.0f, // dtheta_ll误差
leg_argu_[1].theta - 平衡角度, // theta_lr误差
leg_argu_[1].d_theta - 0.0f, // dtheta_lr误差
this->pit_ - 0.0f, // theta_b误差
this->gyro_.x - 0.0f // dtheta_b误差
};
```
#### 4. 修正控制律计算
使用标准的LQR控制律：
```cpp
// 计算控制输出：u = -K * (x - x_ref)
float control[4] = {0}; // [T_wl, T_wr, T_bl, T_br]
for(int i = 0; i < 4; i++) {
for(int j = 0; j < 10; j++) {
control[i] += LQR_K[i*10 + j] * state_error[j];
}
control[i] = -control[i]; // 负反馈
}
leg_argu_[0].Tw = control[0]; // T_wl
leg_argu_[1].Tw = control[1]; // T_wr
leg_argu_[0].Tp = control[2]; // T_bl
leg_argu_[1].Tp = control[3]; // T_br
```
### 总结：
当前的LQR实现在数学原理上有偏差，需要按照标准的LQR控制理论和MATLAB模型进行修正。
主要是要确保状态向量定义、增益矩阵维度和控制律计算都与理论模型一致。

201
MDK-ARM/DevC.uvprojx
View File

@ -11,7 +11,6 @@
<ToolsetNumber>0x4</ToolsetNumber> <ToolsetNumber>0x4</ToolsetNumber>
<ToolsetName>ARM-ADS</ToolsetName> <ToolsetName>ARM-ADS</ToolsetName>
<pArmCC>6160000::V6.16::ARMCLANG</pArmCC> <pArmCC>6160000::V6.16::ARMCLANG</pArmCC>
<pCCUsed>6160000::V6.16::ARMCLANG</pCCUsed>
<uAC6>1</uAC6> <uAC6>1</uAC6>
<TargetOption> <TargetOption>
<TargetCommonOption> <TargetCommonOption>
@ -82,7 +81,7 @@
</BeforeMake> </BeforeMake>
<AfterMake> <AfterMake>
<RunUserProg1>0</RunUserProg1> <RunUserProg1>0</RunUserProg1>
<RunUserProg2>0</RunUserProg2> <RunUserProg2>1</RunUserProg2>
<UserProg1Name></UserProg1Name> <UserProg1Name></UserProg1Name>
<UserProg2Name></UserProg2Name> <UserProg2Name></UserProg2Name>
<UserProg1Dos16Mode>0</UserProg1Dos16Mode> <UserProg1Dos16Mode>0</UserProg1Dos16Mode>
@ -314,7 +313,7 @@
</ArmAdsMisc> </ArmAdsMisc>
<Cads> <Cads>
<interw>1</interw> <interw>1</interw>
<Optim>2</Optim> <Optim>4</Optim>
<oTime>0</oTime> <oTime>0</oTime>
<SplitLS>0</SplitLS> <SplitLS>0</SplitLS>
<OneElfS>1</OneElfS> <OneElfS>1</OneElfS>
@ -340,7 +339,7 @@
<MiscControls></MiscControls> <MiscControls></MiscControls>
<Define>USE_HAL_DRIVER,STM32F407xx</Define> <Define>USE_HAL_DRIVER,STM32F407xx</Define>
<Undefine></Undefine> <Undefine></Undefine>
<IncludePath>../Core/Inc;../USB_DEVICE/App;../USB_DEVICE/Target;../Drivers/STM32F4xx_HAL_Driver/Inc;../Drivers/STM32F4xx_HAL_Driver/Inc/Legacy;../Middlewares/Third_Party/FreeRTOS/Source/include;../Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2;../Middlewares/Third_Party/FreeRTOS/Source/portable/RVDS/ARM_CM4F;../Middlewares/ST/STM32_USB_Device_Library/Core/Inc;../Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Inc;../Drivers/CMSIS/Device/ST/STM32F4xx/Include;../Drivers/CMSIS/Include;..\User</IncludePath> <IncludePath>../Core/Inc;../USB_DEVICE/App;../USB_DEVICE/Target;../Drivers/STM32F4xx_HAL_Driver/Inc;../Drivers/STM32F4xx_HAL_Driver/Inc/Legacy;../Middlewares/Third_Party/FreeRTOS/Source/include;../Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2;../Middlewares/Third_Party/FreeRTOS/Source/portable/RVDS/ARM_CM4F;../Middlewares/ST/STM32_USB_Device_Library/Core/Inc;../Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Inc;../Drivers/CMSIS/Device/ST/STM32F4xx/Include;../Drivers/CMSIS/Include</IncludePath>
</VariousControls> </VariousControls>
</Cads> </Cads>
<Aads> <Aads>
@ -731,191 +730,6 @@
</File> </File>
</Files> </Files>
</Group> </Group>
<Group>
<GroupName>User/bsp</GroupName>
<Files>
<File>
<FileName>can.c</FileName>
<FileType>1</FileType>
<FilePath>..\User\bsp\can.c</FilePath>
</File>
<File>
<FileName>dwt.c</FileName>
<FileType>1</FileType>
<FilePath>..\User\bsp\dwt.c</FilePath>
</File>
<File>
<FileName>gpio.c</FileName>
<FileType>1</FileType>
<FilePath>..\User\bsp\gpio.c</FilePath>
</File>
<File>
<FileName>mm.c</FileName>
<FileType>1</FileType>
<FilePath>..\User\bsp\mm.c</FilePath>
</File>
<File>
<FileName>pwm.c</FileName>
<FileType>1</FileType>
<FilePath>..\User\bsp\pwm.c</FilePath>
</File>
<File>
<FileName>spi.c</FileName>
<FileType>1</FileType>
<FilePath>..\User\bsp\spi.c</FilePath>
</File>
<File>
<FileName>time.c</FileName>
<FileType>1</FileType>
<FilePath>..\User\bsp\time.c</FilePath>
</File>
<File>
<FileName>uart.c</FileName>
<FileType>1</FileType>
<FilePath>..\User\bsp\uart.c</FilePath>
</File>
</Files>
</Group>
<Group>
<GroupName>User/component</GroupName>
<Files>
<File>
<FileName>ahrs.c</FileName>
<FileType>1</FileType>
<FilePath>..\User\component\ahrs.c</FilePath>
</File>
<File>
<FileName>cmd.c</FileName>
<FileType>1</FileType>
<FilePath>..\User\component\cmd.c</FilePath>
</File>
<File>
<FileName>filter.c</FileName>
<FileType>1</FileType>
<FilePath>..\User\component\filter.c</FilePath>
</File>
<File>
<FileName>limiter.c</FileName>
<FileType>1</FileType>
<FilePath>..\User\component\limiter.c</FilePath>
</File>
<File>
<FileName>pid.c</FileName>
<FileType>1</FileType>
<FilePath>..\User\component\pid.c</FilePath>
</File>
<File>
<FileName>user_math.c</FileName>
<FileType>1</FileType>
<FilePath>..\User\component\user_math.c</FilePath>
</File>
<File>
<FileName>kinematics.c</FileName>
<FileType>1</FileType>
<FilePath>..\User\component\kinematics.c</FilePath>
</File>
<File>
<FileName>lqr.c</FileName>
<FileType>1</FileType>
<FilePath>..\User\component\lqr.c</FilePath>
</File>
<File>
<FileName>vmc.c</FileName>
<FileType>1</FileType>
<FilePath>..\User\component\vmc.c</FilePath>
</File>
</Files>
</Group>
<Group>
<GroupName>User/device</GroupName>
<Files>
<File>
<FileName>buzzer.c</FileName>
<FileType>1</FileType>
<FilePath>..\User\device\buzzer.c</FilePath>
</File>
<File>
<FileName>dm_imu.c</FileName>
<FileType>1</FileType>
<FilePath>..\User\device\dm_imu.c</FilePath>
</File>
<File>
<FileName>dr16.c</FileName>
<FileType>1</FileType>
<FilePath>..\User\device\dr16.c</FilePath>
</File>
<File>
<FileName>motor.c</FileName>
<FileType>1</FileType>
<FilePath>..\User\device\motor.c</FilePath>
</File>
<File>
<FileName>motor_lk.c</FileName>
<FileType>1</FileType>
<FilePath>..\User\device\motor_lk.c</FilePath>
</File>
<File>
<FileName>motor_lz.c</FileName>
<FileType>1</FileType>
<FilePath>..\User\device\motor_lz.c</FilePath>
</File>
</Files>
</Group>
<Group>
<GroupName>User/module</GroupName>
<Files>
<File>
<FileName>config.c</FileName>
<FileType>1</FileType>
<FilePath>..\User\module\config.c</FilePath>
</File>
<File>
<FileName>balance_chassis.c</FileName>
<FileType>1</FileType>
<FilePath>..\User\module\balance_chassis.c</FilePath>
</File>
</Files>
</Group>
<Group>
<GroupName>User/task</GroupName>
<Files>
<File>
<FileName>blink.c</FileName>
<FileType>1</FileType>
<FilePath>..\User\task\blink.c</FilePath>
</File>
<File>
<FileName>init.c</FileName>
<FileType>1</FileType>
<FilePath>..\User\task\init.c</FilePath>
</File>
<File>
<FileName>rc.c</FileName>
<FileType>1</FileType>
<FilePath>..\User\task\rc.c</FilePath>
</File>
<File>
<FileName>user_task.c</FileName>
<FileType>1</FileType>
<FilePath>..\User\task\user_task.c</FilePath>
</File>
<File>
<FileName>user_task.h</FileName>
<FileType>5</FileType>
<FilePath>..\User\task\user_task.h</FilePath>
</File>
<File>
<FileName>atti_esti.c</FileName>
<FileType>1</FileType>
<FilePath>..\User\task\atti_esti.c</FilePath>
</File>
<File>
<FileName>ctrl_chassis.c</FileName>
<FileType>1</FileType>
<FilePath>..\User\task\ctrl_chassis.c</FilePath>
</File>
</Files>
</Group>
<Group> <Group>
<GroupName>::CMSIS</GroupName> <GroupName>::CMSIS</GroupName>
</Group> </Group>
@ -936,13 +750,4 @@
<files/> <files/>
</RTE> </RTE>
<LayerInfo>
<Layers>
<Layer>
<LayName>DevC</LayName>
<LayPrjMark>1</LayPrjMark>
</Layer>
</Layers>
</LayerInfo>
</Project> </Project>

31
MDK-ARM/DevC/DevC.lnp
View File

@ -61,37 +61,6 @@
"devc\usbd_ctlreq.o" "devc\usbd_ctlreq.o"
"devc\usbd_ioreq.o" "devc\usbd_ioreq.o"
"devc\usbd_cdc.o" "devc\usbd_cdc.o"
"devc\can_1.o"
"devc\dwt.o"
"devc\gpio_1.o"
"devc\mm.o"
"devc\pwm.o"
"devc\spi_1.o"
"devc\time.o"
"devc\uart.o"
"devc\ahrs.o"
"devc\cmd.o"
"devc\filter.o"
"devc\limiter.o"
"devc\pid.o"
"devc\user_math.o"
"devc\kinematics.o"
"devc\lqr.o"
"devc\vmc.o"
"devc\buzzer.o"
"devc\dm_imu.o"
"devc\dr16.o"
"devc\motor.o"
"devc\motor_lk.o"
"devc\motor_lz.o"
"devc\config.o"
"devc\balance_chassis.o"
"devc\blink.o"
"devc\init.o"
"devc\rc.o"
"devc\user_task.o"
"devc\atti_esti.o"
"devc\ctrl_chassis.o"
--strict --scatter "DevC\DevC.sct" --strict --scatter "DevC\DevC.sct"
--summary_stderr --info summarysizes --map --load_addr_map_info --xref --callgraph --symbols --summary_stderr --info summarysizes --map --load_addr_map_info --xref --callgraph --symbols
--info sizes --info totals --info unused --info veneers --info sizes --info totals --info unused --info veneers

2
MDK-ARM/DevC/ExtDll.iex
View File

@ -1,2 +0,0 @@
[EXTDLL]
Count=0

9
MDK-ARM/EventRecorderStub.scvd
View File

@ -1,9 +0,0 @@
<?xml version="1.0" encoding="utf-8"?>
<component_viewer schemaVersion="0.1" xmlns:xs="http://www.w3.org/2001/XMLSchema-instance" xs:noNamespaceSchemaLocation="Component_Viewer.xsd">
<component name="EventRecorderStub" version="1.0.0"/> <!--name and version of the component-->
<events>
</events>
</component_viewer>

297
User/README.md
View File

@ -1,297 +0,0 @@
# 轮腿机器人LQR平衡控制系统
这是一个完整的轮腿机器人LQR+VMC平衡控制系统，支持6个电机（2个轮电机+4个关节电机）的协调控制。
## 系统架构
```
┌─────────────────┐ ┌──────────────────┐ ┌─────────────────┐
│ 传感器输入 │ │ 平衡控制器 │ │ 电机控制输出 │
│ │ │ │ │ │
│ • IMU姿态数据 │────▶│ LQR控制器 │────▶│ • 轮电机力矩 │
│ • 电机反馈数据 │ │ VMC控制器 │ │ • 关节电机力矩 │
│ • 遥控器命令 │ │ 状态估计器 │ │ • MIT控制参数 │
└─────────────────┘ └──────────────────┘ └─────────────────┘
```
## 文件结构
### 核心控制文件
- `balance_control.h/c` - 主控制器，整合LQR和VMC
- `lqr.h/c` - LQR控制器实现
- `vmc.h/c` - 虚拟模型控制器
- `kinematics.h/c` - 运动学计算
### 使用示例
- `balance_control_example.c` - 完整的使用示例
### MATLAB工具
- `lqr_design_optimized.m` - LQR参数设计和系数生成
- `vmc.m` - VMC控制器设计参考
## 快速开始
### 1. 硬件连接
确保以下硬件正确连接：
- IMU传感器（提供机体姿态）
- 2个轮电机（支持转矩电流控制）
- 4个关节电机（支持MIT控制模式）
- CAN总线通信
### 2. 参数配置
#### 2.1 运行MATLAB脚本生成LQR参数
```matlab
% 在MATLAB中运行
run('utils/lqr_design_optimized.m');
```
这将生成K矩阵的拟合系数，复制到`lqr.c`中的`K_fit_coefficients`数组。
#### 2.2 修改机器人物理参数
在`lqr.h`中修改您的机器人参数：
```c
#define R_W 0.09f // 驱动轮半径 (m)
#define R_L 0.25f // 两个驱动轮之间距离/2 (m)
#define M_W 0.8f // 驱动轮质量 (kg)
#define M_L 1.6183599f // 腿部质量 (kg)
#define M_B 11.542f // 机体质量 (kg)
// ... 其他参数
```
#### 2.3 配置电机ID和CAN接口
在`balance_control_example.c`中修改电机ID：
```c
#define WHEEL_LEFT_ID 0x01
#define WHEEL_RIGHT_ID 0x02
#define HIP_LEFT_ID 0x03
#define HIP_RIGHT_ID 0x04
#define KNEE_LEFT_ID 0x05
#define KNEE_RIGHT_ID 0x06
```
### 3. 代码集成
#### 3.1 在main函数中初始化
```c
#include "balance_control.h"
int main(void)
{
// HAL初始化
HAL_Init();
SystemClock_Config();
// 初始化外设
MX_CAN1_Init();
MX_TIM1_Init();
// 初始化平衡控制系统
balance_system_init();
// 启动控制循环
HAL_TIM_Base_Start_IT(&htim1); // 1kHz控制频率
while(1)
{
// 主循环处理其他任务
HAL_Delay(100);
}
}
```
#### 3.2 定时器中断处理
```c
void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
{
if (htim->Instance == TIM1) {
balance_control_task(); // 1kHz控制任务
}
}
```
### 4. 传感器数据接口
#### 4.1 实现IMU数据读取
在`balance_control_example.c`中实现`read_imu_data()`函数：
```c
void read_imu_data(imu_data_t* imu)
{
// 从您的IMU传感器读取数据
imu->pitch = get_imu_pitch();
imu->roll = get_imu_roll();
imu->yaw = get_imu_yaw();
imu->pitch_rate = get_imu_pitch_rate();
imu->roll_rate = get_imu_roll_rate();
imu->yaw_rate = get_imu_yaw_rate();
// ... 其他数据
}
```
#### 4.2 实现电机反馈读取
```c
void read_motor_feedback(motor_feedback_t* motor_fb)
{
// 从CAN总线读取电机反馈
motor_fb->wheel_left_angle = get_motor_angle(WHEEL_LEFT_ID);
motor_fb->wheel_right_angle = get_motor_angle(WHEEL_RIGHT_ID);
motor_fb->hip_left_angle = get_motor_angle(HIP_LEFT_ID);
// ... 其他电机数据
}
```
#### 4.3 实现电机控制输出
```c
void send_motor_control(const motor_control_t* motor_ctrl)
{
// 发送轮电机转矩控制指令
send_wheel_torque_cmd(WHEEL_LEFT_ID, motor_ctrl->wheel_left_torque_cmd);
send_wheel_torque_cmd(WHEEL_RIGHT_ID, motor_ctrl->wheel_right_torque_cmd);
// 发送关节电机MIT控制指令
send_mit_control_cmd(HIP_LEFT_ID,
0, // 位置指令
0, // 速度指令
motor_ctrl->hip_left_kp,
motor_ctrl->hip_left_kd,
motor_ctrl->hip_left_torque_cmd);
// ... 其他关节电机
}
```
## 控制模式
### 1. 平衡模式 (ROBOT_STATE_BALANCE)
- 自动保持机体平衡
- 响应遥控器前进/转向命令
- 自动调节腿长以适应地形
### 2. 运动模式 (ROBOT_STATE_MOVE)
- 在平衡基础上执行运动控制
- 支持前进、后退、转向
- 支持高度调节
### 3. 紧急停止 (ROBOT_STATE_EMERGENCY)
- 立即停止所有电机输出
- 安全保护机制
## 控制参数调节
### LQR参数调节
在MATLAB脚本`lqr_design_optimized.m`中修改Q和R矩阵：
```matlab
% 状态权重矩阵Q - 增大数值提高控制精度
Q = diag([
10, % s - 水平位移
5, % ds - 水平速度
20, % phi - 偏航角
10, % dphi - 偏航角速度
100, % theta_ll - 左腿角
50, % dtheta_ll - 左腿角速度
100, % theta_lr - 右腿角
50, % dtheta_lr - 右腿角速度
200, % theta_b - 机体倾斜角 (最重要)
100 % dtheta_b - 机体角速度
]);
% 控制权重矩阵R - 增大数值降低控制量
R = diag([
0.1, % T_wl - 左轮力矩
0.1, % T_wr - 右轮力矩
1.0, % T_bl - 左腿力矩
1.0 % T_br - 右腿力矩
]);
```
### VMC参数调节
在`balance_control.c`中修改VMC参数：
```c
vmc_params_t vmc_params = {
.K_spring = 800.0f, // 径向弹簧刚度 - 控制腿长刚度
.K_damper = 80.0f, // 径向阻尼系数 - 控制腿长阻尼
.K_theta = 150.0f, // 角度刚度 - 控制腿角刚度
.K_dtheta = 15.0f, // 角速度阻尼 - 控制腿角阻尼
.max_radial_force = 1000.0f, // 最大径向力
.max_tangential_force = 500.0f, // 最大切向力
};
```
## 安全机制
### 1. 倾斜角保护
```c
#define MAX_TILT_ANGLE 0.5f // 最大倾斜角 (rad)
```
### 2. 腿长保护
```c
#define LEG_MIN_REACH 0.05f // 最小腿长 (m)
#define LEG_MAX_REACH 0.28f // 最大腿长 (m)
```
### 3. 力矩限制
```c
#define MAX_WHEEL_TORQUE 50.0f // 最大轮子力矩 (N*m)
#define MAX_JOINT_TORQUE 100.0f // 最大关节力矩 (N*m)
```
## 调试和监控
### 1. 串口调试输出
系统会定期输出关键状态信息：
```
=== Balance Control Status ===
Task Count: 1000
Robot State: 1
Safety Flag: 1
IMU Pitch: 0.050 rad (2.9 deg)
Left Leg Length: 0.200 m
Right Leg Length: 0.200 m
==============================
```
### 2. 状态监控
可以通过以下变量监控系统状态：
- `g_balance_ctrl.state` - 机器人状态
- `g_balance_ctrl.safety_flag` - 安全标志
- `g_balance_ctrl.robot_state` - 完整机器人状态
## 常见问题
### Q: 机器人启动后不稳定？
A:
1. 检查IMU标定是否正确
2. 调整LQR的Q矩阵，增大机体角度权重
3. 检查电机正负方向是否正确
### Q: 腿部抖动？
A:
1. 降低VMC的刚度参数
2. 增加VMC的阻尼参数
3. 检查关节电机的刚度设置
### Q: 轮子打滑？
A:
1. 降低LQR的轮子力矩输出
2. 增加R矩阵中轮子力矩的权重
3. 检查地面摩擦条件
### Q: 系统响应慢？
A:
1. 增加控制频率（目前为1kHz）
2. 调整LQR的Q矩阵权重
3. 检查CAN通信延迟
## 技术支持
如需技术支持或有疑问，请：
1. 检查上述常见问题
2. 确认硬件连接正确
3. 验证参数配置无误
4. 查看串口调试输出
## 更新日志
- v1.0 (2025-08-30): 初始版本，支持LQR+VMC协调控制
- 完整的6电机控制接口
- MATLAB参数设计工具
- 完善的安全保护机制

16
User/bsp/bsp.h
View File

@ -1,16 +0,0 @@
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
#define BSP_OK (0)
#define BSP_ERR (-1)
#define BSP_ERR_NULL (-2)
#define BSP_ERR_INITED (-3)
#define BSP_ERR_NO_DEV (-4)
#define BSP_ERR_TIMEOUT (-5)
#ifdef __cplusplus
}
#endif

132
User/bsp/bsp_config.yaml
View File

@ -1,132 +0,0 @@
can:
devices:
- instance: CAN1
name: '1'
- instance: CAN2
name: '2'
enabled: true
dwt:
enabled: true
gpio:
configs:
- custom_name: USER_KEY
has_exti: true
ioc_label: USER_KEY
pin: PA0-WKUP
type: EXTI
- custom_name: ACCL_CS
has_exti: false
ioc_label: ACCL_CS
pin: PA4
type: OUTPUT
- custom_name: GYRO_CS
has_exti: false
ioc_label: GYRO_CS
pin: PB0
type: OUTPUT
- custom_name: SPI2_CS
has_exti: false
ioc_label: SPI2_CS
pin: PB12
type: OUTPUT
- custom_name: HW0
has_exti: false
ioc_label: HW0
pin: PC0
type: INPUT
- custom_name: HW1
has_exti: false
ioc_label: HW1
pin: PC1
type: INPUT
- custom_name: HW2
has_exti: false
ioc_label: HW2
pin: PC2
type: INPUT
- custom_name: ACCL_INT
has_exti: true
ioc_label: ACCL_INT
pin: PC4
type: EXTI
- custom_name: GYRO_INT
has_exti: true
ioc_label: GYRO_INT
pin: PC5
type: EXTI
- custom_name: CMPS_INT
has_exti: true
ioc_label: CMPS_INT
pin: PG3
type: EXTI
- custom_name: CMPS_RST
has_exti: false
ioc_label: CMPS_RST
pin: PG6
type: OUTPUT
enabled: true
mm:
enabled: true
pwm:
configs:
- channel: TIM_CHANNEL_1
custom_name: TIM8_CH1
label: TIM8_CH1
timer: TIM8
- channel: TIM_CHANNEL_3
custom_name: LASER
label: LASER
timer: TIM3
- channel: TIM_CHANNEL_3
custom_name: BUZZER
label: BUZZER
timer: TIM4
- channel: TIM_CHANNEL_2
custom_name: TIM1_CH2
label: TIM1_CH2
timer: TIM1
- channel: TIM_CHANNEL_3
custom_name: TIM1_CH3
label: TIM1_CH3
timer: TIM1
- channel: TIM_CHANNEL_4
custom_name: TIM1_CH4
label: TIM1_CH4
timer: TIM1
- channel: TIM_CHANNEL_1
custom_name: TIM1_CH1
label: TIM1_CH1
timer: TIM1
- channel: TIM_CHANNEL_1
custom_name: IMU_HEAT_PWM
label: IMU_HEAT_PWM
timer: TIM10
- channel: TIM_CHANNEL_1
custom_name: LED_B
label: LED_B
timer: TIM5
- channel: TIM_CHANNEL_2
custom_name: LED_G
label: LED_G
timer: TIM5
- channel: TIM_CHANNEL_3
custom_name: LED_R
label: LED_R
timer: TIM5
- channel: TIM_CHANNEL_2
custom_name: TIM8_CH2
label: TIM8_CH2
timer: TIM8
enabled: true
spi:
devices:
- instance: SPI1
name: BMI088
enabled: true
time:
enabled: true
uart:
devices:
- instance: USART3
name: DR16
enabled: true

701
User/bsp/can.c
View File

@ -1,701 +0,0 @@
/* Includes ----------------------------------------------------------------- */
#include "bsp/can.h"
#include "bsp/bsp.h"
#include <can.h>
#include <cmsis_os.h>
#include <string.h>
/* Private define ----------------------------------------------------------- */
#define CAN_QUEUE_MUTEX_TIMEOUT 100 /* 队列互斥锁超时时间(ms) */
#define CAN_TX_SEMAPHORE_TIMEOUT 1000 /* 发送信号量超时时间(ms) */
#define CAN_TX_MAILBOX_NUM 3 /* CAN发送邮箱数量 */
/* Private macro ------------------------------------------------------------ */
/* Private typedef ---------------------------------------------------------- */
typedef struct BSP_CAN_QueueNode {
BSP_CAN_t can; /* CAN通道 */
uint32_t can_id; /* 解析后的CAN ID */
osMessageQueueId_t queue; /* 消息队列ID */
uint8_t queue_size; /* 队列大小 */
struct BSP_CAN_QueueNode *next; /* 指向下一个节点的指针 */
} BSP_CAN_QueueNode_t;
/* Private variables -------------------------------------------------------- */
static BSP_CAN_QueueNode_t *queue_list = NULL;
static osMutexId_t queue_mutex = NULL;
static osSemaphoreId_t tx_semaphore[BSP_CAN_NUM] = {NULL}; /* 发送信号量，用于控制发送邮箱访问 */
static void (*CAN_Callback[BSP_CAN_NUM][BSP_CAN_CB_NUM])(void);
static bool inited = false;
static BSP_CAN_IdParser_t id_parser = NULL; /* ID解析器 */
/* Private function prototypes ---------------------------------------------- */
static BSP_CAN_t CAN_Get(CAN_HandleTypeDef *hcan);
static osMessageQueueId_t BSP_CAN_FindQueue(BSP_CAN_t can, uint32_t can_id);
static int8_t BSP_CAN_CreateIdQueue(BSP_CAN_t can, uint32_t can_id, uint8_t queue_size);
static int8_t BSP_CAN_DeleteIdQueue(BSP_CAN_t can, uint32_t can_id);
static void BSP_CAN_RxFifo0Callback(void);
static void BSP_CAN_RxFifo1Callback(void);
static BSP_CAN_FrameType_t BSP_CAN_GetFrameType(CAN_RxHeaderTypeDef *header);
static uint32_t BSP_CAN_DefaultIdParser(uint32_t original_id, BSP_CAN_FrameType_t frame_type);
/* Private functions -------------------------------------------------------- */
/**
* @brief 根据CAN句柄获取BSP_CAN实例
*/
static BSP_CAN_t CAN_Get(CAN_HandleTypeDef *hcan) {
if (hcan == NULL) return BSP_CAN_ERR;
if (hcan->Instance == CAN1)
return BSP_CAN_1;
else if (hcan->Instance == CAN2)
return BSP_CAN_2;
else
return BSP_CAN_ERR;
}
/**
* @brief 查找指定CAN ID的消息队列
* @note 调用前需要获取互斥锁
*/
static osMessageQueueId_t BSP_CAN_FindQueue(BSP_CAN_t can, uint32_t can_id) {
BSP_CAN_QueueNode_t *node = queue_list;
while (node != NULL) {
if (node->can == can && node->can_id == can_id) {
return node->queue;
}
node = node->next;
}
return NULL;
}
/**
* @brief 创建指定CAN ID的消息队列
* @note 内部函数，已包含互斥锁保护
*/
static int8_t BSP_CAN_CreateIdQueue(BSP_CAN_t can, uint32_t can_id, uint8_t queue_size) {
if (queue_size == 0) {
queue_size = BSP_CAN_DEFAULT_QUEUE_SIZE;
}
if (osMutexAcquire(queue_mutex, CAN_QUEUE_MUTEX_TIMEOUT) != osOK) {
return BSP_ERR_TIMEOUT;
}
BSP_CAN_QueueNode_t *node = queue_list;
while (node != NULL) {
if (node->can == can && node->can_id == can_id) {
osMutexRelease(queue_mutex);
return BSP_ERR; // 已存在
}
node = node->next;
}
BSP_CAN_QueueNode_t *new_node = (BSP_CAN_QueueNode_t *)BSP_Malloc(sizeof(BSP_CAN_QueueNode_t));
if (new_node == NULL) {
osMutexRelease(queue_mutex);
return BSP_ERR_NULL;
}
new_node->queue = osMessageQueueNew(queue_size, sizeof(BSP_CAN_Message_t), NULL);
if (new_node->queue == NULL) {
BSP_Free(new_node);
osMutexRelease(queue_mutex);
return BSP_ERR;
}
new_node->can = can;
new_node->can_id = can_id;
new_node->queue_size = queue_size;
new_node->next = queue_list;
queue_list = new_node;
osMutexRelease(queue_mutex);
return BSP_OK;
}
/**
* @brief 删除指定CAN ID的消息队列
* @note 内部函数，已包含互斥锁保护
*/
static int8_t BSP_CAN_DeleteIdQueue(BSP_CAN_t can, uint32_t can_id) {
if (osMutexAcquire(queue_mutex, CAN_QUEUE_MUTEX_TIMEOUT) != osOK) {
return BSP_ERR_TIMEOUT;
}
BSP_CAN_QueueNode_t **current = &queue_list;
while (*current != NULL) {
if ((*current)->can == can && (*current)->can_id == can_id) {
BSP_CAN_QueueNode_t *to_delete = *current;
*current = (*current)->next;
osMessageQueueDelete(to_delete->queue);
BSP_Free(to_delete);
osMutexRelease(queue_mutex);
return BSP_OK;
}
current = &(*current)->next;
}
osMutexRelease(queue_mutex);
return BSP_ERR; // 未找到
}
/**
* @brief 获取帧类型
*/
static BSP_CAN_FrameType_t BSP_CAN_GetFrameType(CAN_RxHeaderTypeDef *header) {
if (header->RTR == CAN_RTR_REMOTE) {
return (header->IDE == CAN_ID_EXT) ? BSP_CAN_FRAME_EXT_REMOTE : BSP_CAN_FRAME_STD_REMOTE;
} else {
return (header->IDE == CAN_ID_EXT) ? BSP_CAN_FRAME_EXT_DATA : BSP_CAN_FRAME_STD_DATA;
}
}
/**
* @brief 默认ID解析器（直接返回原始ID）
*/
static uint32_t BSP_CAN_DefaultIdParser(uint32_t original_id, BSP_CAN_FrameType_t frame_type) {
(void)frame_type; // 避免未使用参数警告
return original_id;
}
/**
* @brief FIFO0接收处理函数
*/
static void BSP_CAN_RxFifo0Callback(void) {
CAN_RxHeaderTypeDef rx_header;
uint8_t rx_data[BSP_CAN_MAX_DLC];
for (int can_idx = 0; can_idx < BSP_CAN_NUM; can_idx++) {
CAN_HandleTypeDef *hcan = BSP_CAN_GetHandle((BSP_CAN_t)can_idx);
if (hcan == NULL) continue;
while (HAL_CAN_GetRxFifoFillLevel(hcan, CAN_RX_FIFO0) > 0) {
if (HAL_CAN_GetRxMessage(hcan, CAN_RX_FIFO0, &rx_header, rx_data) == HAL_OK) {
uint32_t original_id = (rx_header.IDE == CAN_ID_STD) ? rx_header.StdId : rx_header.ExtId;
BSP_CAN_FrameType_t frame_type = BSP_CAN_GetFrameType(&rx_header);
uint32_t parsed_id = BSP_CAN_ParseId(original_id, frame_type);
osMessageQueueId_t queue = BSP_CAN_FindQueue((BSP_CAN_t)can_idx, parsed_id);
if (queue != NULL) {
BSP_CAN_Message_t msg = {0};
msg.frame_type = frame_type;
msg.original_id = original_id;
msg.parsed_id = parsed_id;
msg.dlc = rx_header.DLC;
if (rx_header.RTR == CAN_RTR_DATA) {
memcpy(msg.data, rx_data, rx_header.DLC);
}
msg.timestamp = HAL_GetTick();
osMessageQueuePut(queue, &msg, 0, BSP_CAN_TIMEOUT_IMMEDIATE);
}
}
}
}
}
/**
* @brief FIFO1接收处理函数
*/
static void BSP_CAN_RxFifo1Callback(void) {
CAN_RxHeaderTypeDef rx_header;
uint8_t rx_data[BSP_CAN_MAX_DLC];
for (int can_idx = 0; can_idx < BSP_CAN_NUM; can_idx++) {
CAN_HandleTypeDef *hcan = BSP_CAN_GetHandle((BSP_CAN_t)can_idx);
if (hcan == NULL) continue;
while (HAL_CAN_GetRxFifoFillLevel(hcan, CAN_RX_FIFO1) > 0) {
if (HAL_CAN_GetRxMessage(hcan, CAN_RX_FIFO1, &rx_header, rx_data) == HAL_OK) {
uint32_t original_id = (rx_header.IDE == CAN_ID_STD) ? rx_header.StdId : rx_header.ExtId;
BSP_CAN_FrameType_t frame_type = BSP_CAN_GetFrameType(&rx_header);
uint32_t parsed_id = BSP_CAN_ParseId(original_id, frame_type);
osMessageQueueId_t queue = BSP_CAN_FindQueue((BSP_CAN_t)can_idx, parsed_id);
if (queue != NULL) {
BSP_CAN_Message_t msg = {0};
msg.frame_type = frame_type;
msg.original_id = original_id;
msg.parsed_id = parsed_id;
msg.dlc = rx_header.DLC;
if (rx_header.RTR == CAN_RTR_DATA) {
memcpy(msg.data, rx_data, rx_header.DLC);
}
msg.timestamp = HAL_GetTick();
osMessageQueuePut(queue, &msg, 0, BSP_CAN_TIMEOUT_IMMEDIATE);
}
}
}
}
}
/* HAL Callback Functions --------------------------------------------------- */
void HAL_CAN_TxMailbox0CompleteCallback(CAN_HandleTypeDef *hcan) {
BSP_CAN_t bsp_can = CAN_Get(hcan);
if (bsp_can != BSP_CAN_ERR) {
// 释放发送信号量
if (tx_semaphore[bsp_can] != NULL) {
osSemaphoreRelease(tx_semaphore[bsp_can]);
}
if (CAN_Callback[bsp_can][HAL_CAN_TX_MAILBOX0_CPLT_CB])
CAN_Callback[bsp_can][HAL_CAN_TX_MAILBOX0_CPLT_CB]();
}
}
void HAL_CAN_TxMailbox1CompleteCallback(CAN_HandleTypeDef *hcan) {
BSP_CAN_t bsp_can = CAN_Get(hcan);
if (bsp_can != BSP_CAN_ERR) {
// 释放发送信号量
if (tx_semaphore[bsp_can] != NULL) {
osSemaphoreRelease(tx_semaphore[bsp_can]);
}
if (CAN_Callback[bsp_can][HAL_CAN_TX_MAILBOX1_CPLT_CB])
CAN_Callback[bsp_can][HAL_CAN_TX_MAILBOX1_CPLT_CB]();
}
}
void HAL_CAN_TxMailbox2CompleteCallback(CAN_HandleTypeDef *hcan) {
BSP_CAN_t bsp_can = CAN_Get(hcan);
if (bsp_can != BSP_CAN_ERR) {
// 释放发送信号量
if (tx_semaphore[bsp_can] != NULL) {
osSemaphoreRelease(tx_semaphore[bsp_can]);
}
if (CAN_Callback[bsp_can][HAL_CAN_TX_MAILBOX2_CPLT_CB])
CAN_Callback[bsp_can][HAL_CAN_TX_MAILBOX2_CPLT_CB]();
}
}
void HAL_CAN_TxMailbox0AbortCallback(CAN_HandleTypeDef *hcan) {
BSP_CAN_t bsp_can = CAN_Get(hcan);
if (bsp_can != BSP_CAN_ERR) {
// 释放发送信号量（发送中止也要释放）
if (tx_semaphore[bsp_can] != NULL) {
osSemaphoreRelease(tx_semaphore[bsp_can]);
}
if (CAN_Callback[bsp_can][HAL_CAN_TX_MAILBOX0_ABORT_CB])
CAN_Callback[bsp_can][HAL_CAN_TX_MAILBOX0_ABORT_CB]();
}
}
void HAL_CAN_TxMailbox1AbortCallback(CAN_HandleTypeDef *hcan) {
BSP_CAN_t bsp_can = CAN_Get(hcan);
if (bsp_can != BSP_CAN_ERR) {
// 释放发送信号量（发送中止也要释放）
if (tx_semaphore[bsp_can] != NULL) {
osSemaphoreRelease(tx_semaphore[bsp_can]);
}
if (CAN_Callback[bsp_can][HAL_CAN_TX_MAILBOX1_ABORT_CB])
CAN_Callback[bsp_can][HAL_CAN_TX_MAILBOX1_ABORT_CB]();
}
}
void HAL_CAN_TxMailbox2AbortCallback(CAN_HandleTypeDef *hcan) {
BSP_CAN_t bsp_can = CAN_Get(hcan);
if (bsp_can != BSP_CAN_ERR) {
// 释放发送信号量（发送中止也要释放）
if (tx_semaphore[bsp_can] != NULL) {
osSemaphoreRelease(tx_semaphore[bsp_can]);
}
if (CAN_Callback[bsp_can][HAL_CAN_TX_MAILBOX2_ABORT_CB])
CAN_Callback[bsp_can][HAL_CAN_TX_MAILBOX2_ABORT_CB]();
}
}
void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *hcan) {
BSP_CAN_t bsp_can = CAN_Get(hcan);
if (bsp_can != BSP_CAN_ERR) {
if (CAN_Callback[bsp_can][HAL_CAN_RX_FIFO0_MSG_PENDING_CB])
CAN_Callback[bsp_can][HAL_CAN_RX_FIFO0_MSG_PENDING_CB]();
}
}
void HAL_CAN_RxFifo0FullCallback(CAN_HandleTypeDef *hcan) {
BSP_CAN_t bsp_can = CAN_Get(hcan);
if (bsp_can != BSP_CAN_ERR) {
if (CAN_Callback[bsp_can][HAL_CAN_RX_FIFO0_FULL_CB])
CAN_Callback[bsp_can][HAL_CAN_RX_FIFO0_FULL_CB]();
}
}
void HAL_CAN_RxFifo1MsgPendingCallback(CAN_HandleTypeDef *hcan) {
BSP_CAN_t bsp_can = CAN_Get(hcan);
if (bsp_can != BSP_CAN_ERR) {
if (CAN_Callback[bsp_can][HAL_CAN_RX_FIFO1_MSG_PENDING_CB])
CAN_Callback[bsp_can][HAL_CAN_RX_FIFO1_MSG_PENDING_CB]();
}
}
void HAL_CAN_RxFifo1FullCallback(CAN_HandleTypeDef *hcan) {
BSP_CAN_t bsp_can = CAN_Get(hcan);
if (bsp_can != BSP_CAN_ERR) {
if (CAN_Callback[bsp_can][HAL_CAN_RX_FIFO1_FULL_CB])
CAN_Callback[bsp_can][HAL_CAN_RX_FIFO1_FULL_CB]();
}
}
void HAL_CAN_SleepCallback(CAN_HandleTypeDef *hcan) {
BSP_CAN_t bsp_can = CAN_Get(hcan);
if (bsp_can != BSP_CAN_ERR) {
if (CAN_Callback[bsp_can][HAL_CAN_SLEEP_CB])
CAN_Callback[bsp_can][HAL_CAN_SLEEP_CB]();
}
}
void HAL_CAN_WakeUpFromRxMsgCallback(CAN_HandleTypeDef *hcan) {
BSP_CAN_t bsp_can = CAN_Get(hcan);
if (bsp_can != BSP_CAN_ERR) {
if (CAN_Callback[bsp_can][HAL_CAN_WAKEUP_FROM_RX_MSG_CB])
CAN_Callback[bsp_can][HAL_CAN_WAKEUP_FROM_RX_MSG_CB]();
}
}
void HAL_CAN_ErrorCallback(CAN_HandleTypeDef *hcan) {
BSP_CAN_t bsp_can = CAN_Get(hcan);
if (bsp_can != BSP_CAN_ERR) {
if (CAN_Callback[bsp_can][HAL_CAN_ERROR_CB])
CAN_Callback[bsp_can][HAL_CAN_ERROR_CB]();
}
}
/* Exported functions ------------------------------------------------------- */
int8_t BSP_CAN_Init(void) {
if (inited) {
return BSP_ERR_INITED;
}
// 清零回调函数数组
memset(CAN_Callback, 0, sizeof(CAN_Callback));
// 初始化ID解析器为默认解析器
id_parser = BSP_CAN_DefaultIdParser;
// 创建互斥锁
queue_mutex = osMutexNew(NULL);
if (queue_mutex == NULL) {
return BSP_ERR;
}
// 创建发送信号量，每个CAN通道有3个发送邮箱
for (int i = 0; i < BSP_CAN_NUM; i++) {
tx_semaphore[i] = osSemaphoreNew(CAN_TX_MAILBOX_NUM, CAN_TX_MAILBOX_NUM, NULL);
if (tx_semaphore[i] == NULL) {
// 清理已创建的信号量
for (int j = 0; j < i; j++) {
if (tx_semaphore[j] != NULL) {
osSemaphoreDelete(tx_semaphore[j]);
tx_semaphore[j] = NULL;
}
}
if (queue_mutex != NULL) {
osMutexDelete(queue_mutex);
queue_mutex = NULL;
}
return BSP_ERR;
}
}
// 先设置初始化标志，以便后续回调注册能通过检查
inited = true;
// 初始化 CAN1 - 使用 FIFO0
CAN_FilterTypeDef can1_filter = {0};
can1_filter.FilterBank = 0;
can1_filter.FilterIdHigh = 0;
can1_filter.FilterIdLow = 0;
can1_filter.FilterMode = CAN_FILTERMODE_IDMASK;
can1_filter.FilterScale = CAN_FILTERSCALE_32BIT;
can1_filter.FilterMaskIdHigh = 0;
can1_filter.FilterMaskIdLow = 0;
can1_filter.FilterActivation = ENABLE;
can1_filter.SlaveStartFilterBank = 14;
can1_filter.FilterFIFOAssignment = CAN_RX_FIFO0;
HAL_CAN_ConfigFilter(&hcan1, &can1_filter);
HAL_CAN_Start(&hcan1);
// 注册CAN1回调函数
BSP_CAN_RegisterCallback(BSP_CAN_1, HAL_CAN_RX_FIFO0_MSG_PENDING_CB, BSP_CAN_RxFifo0Callback);
HAL_CAN_ActivateNotification(&hcan1, CAN_IT_RX_FIFO0_MSG_PENDING |
CAN_IT_TX_MAILBOX_EMPTY); // 激活发送邮箱空中断
// 初始化 CAN2 - 使用 FIFO1
can1_filter.FilterBank = 14;
can1_filter.FilterFIFOAssignment = CAN_RX_FIFO1;
HAL_CAN_ConfigFilter(&hcan2, &can1_filter); // 通过 CAN1 配置
HAL_CAN_Start(&hcan2);
// 注册CAN2回调函数
BSP_CAN_RegisterCallback(BSP_CAN_2, HAL_CAN_RX_FIFO1_MSG_PENDING_CB, BSP_CAN_RxFifo1Callback);
HAL_CAN_ActivateNotification(&hcan2, CAN_IT_RX_FIFO1_MSG_PENDING |
CAN_IT_TX_MAILBOX_EMPTY); // 激活发送邮箱空中断
inited = true;
return BSP_OK;
}
int8_t BSP_CAN_DeInit(void) {
if (!inited) {
return BSP_ERR;
}
// 删除所有队列
if (osMutexAcquire(queue_mutex, CAN_QUEUE_MUTEX_TIMEOUT) == osOK) {
BSP_CAN_QueueNode_t *current = queue_list;
while (current != NULL) {
BSP_CAN_QueueNode_t *next = current->next;
osMessageQueueDelete(current->queue);
BSP_Free(current);
current = next;
}
queue_list = NULL;
osMutexRelease(queue_mutex);
}
// 删除发送信号量
for (int i = 0; i < BSP_CAN_NUM; i++) {
if (tx_semaphore[i] != NULL) {
osSemaphoreDelete(tx_semaphore[i]);
tx_semaphore[i] = NULL;
}
}
// 删除互斥锁
if (queue_mutex != NULL) {
osMutexDelete(queue_mutex);
queue_mutex = NULL;
}
// 清零回调函数数组
memset(CAN_Callback, 0, sizeof(CAN_Callback));
// 重置ID解析器
id_parser = NULL;
inited = false;
return BSP_OK;
}
CAN_HandleTypeDef *BSP_CAN_GetHandle(BSP_CAN_t can) {
if (can >= BSP_CAN_NUM) {
return NULL;
}
switch (can) {
case BSP_CAN_1:
return &hcan1;
case BSP_CAN_2:
return &hcan2;
default:
return NULL;
}
}
int8_t BSP_CAN_RegisterCallback(BSP_CAN_t can, BSP_CAN_Callback_t type,
void (*callback)(void)) {
if (!inited) {
return BSP_ERR_INITED;
}
if (callback == NULL) {
return BSP_ERR_NULL;
}
if (can >= BSP_CAN_NUM) {
return BSP_ERR;
}
if (type >= BSP_CAN_CB_NUM) {
return BSP_ERR;
}
CAN_Callback[can][type] = callback;
return BSP_OK;
}
int8_t BSP_CAN_Transmit(BSP_CAN_t can, BSP_CAN_Format_t format,
uint32_t id, uint8_t *data, uint8_t dlc) {
if (!inited) {
return BSP_ERR_INITED;
}
if (can >= BSP_CAN_NUM) {
return BSP_ERR;
}
if (data == NULL && format != BSP_CAN_FORMAT_STD_REMOTE && format != BSP_CAN_FORMAT_EXT_REMOTE) {
return BSP_ERR_NULL;
}
if (dlc > BSP_CAN_MAX_DLC) {
return BSP_ERR;
}
// 获取发送信号量，确保有可用的发送邮箱
if (tx_semaphore[can] == NULL) {
return BSP_ERR;
}
osStatus_t sem_status = osSemaphoreAcquire(tx_semaphore[can], CAN_TX_SEMAPHORE_TIMEOUT);
if (sem_status != osOK) {
return BSP_ERR_TIMEOUT; // 获取信号量超时，表示发送邮箱已满
}
CAN_HandleTypeDef *hcan = BSP_CAN_GetHandle(can);
if (hcan == NULL) {
// 如果获取句柄失败，需要释放信号量
osSemaphoreRelease(tx_semaphore[can]);
return BSP_ERR_NULL;
}
CAN_TxHeaderTypeDef header = {0};
uint32_t mailbox;
switch (format) {
case BSP_CAN_FORMAT_STD_DATA:
header.StdId = id;
header.IDE = CAN_ID_STD;
header.RTR = CAN_RTR_DATA;
break;
case BSP_CAN_FORMAT_EXT_DATA:
header.ExtId = id;
header.IDE = CAN_ID_EXT;
header.RTR = CAN_RTR_DATA;
break;
case BSP_CAN_FORMAT_STD_REMOTE:
header.StdId = id;
header.IDE = CAN_ID_STD;
header.RTR = CAN_RTR_REMOTE;
break;
case BSP_CAN_FORMAT_EXT_REMOTE:
header.ExtId = id;
header.IDE = CAN_ID_EXT;
header.RTR = CAN_RTR_REMOTE;
break;
default:
// 如果格式错误，需要释放信号量
osSemaphoreRelease(tx_semaphore[can]);
return BSP_ERR;
}
header.DLC = dlc;
header.TransmitGlobalTime = DISABLE;
HAL_StatusTypeDef result = HAL_CAN_AddTxMessage(hcan, &header, data, &mailbox);
if (result != HAL_OK) {
// 如果发送失败，需要释放信号量
osSemaphoreRelease(tx_semaphore[can]);
return BSP_ERR;
}
// 发送成功，信号量将在发送完成回调中释放
return BSP_OK;
}
int8_t BSP_CAN_TransmitStdDataFrame(BSP_CAN_t can, BSP_CAN_StdDataFrame_t *frame) {
if (frame == NULL) {
return BSP_ERR_NULL;
}
return BSP_CAN_Transmit(can, BSP_CAN_FORMAT_STD_DATA, frame->id, frame->data, frame->dlc);
}
int8_t BSP_CAN_TransmitExtDataFrame(BSP_CAN_t can, BSP_CAN_ExtDataFrame_t *frame) {
if (frame == NULL) {
return BSP_ERR_NULL;
}
return BSP_CAN_Transmit(can, BSP_CAN_FORMAT_EXT_DATA, frame->id, frame->data, frame->dlc);
}
int8_t BSP_CAN_TransmitRemoteFrame(BSP_CAN_t can, BSP_CAN_RemoteFrame_t *frame) {
if (frame == NULL) {
return BSP_ERR_NULL;
}
BSP_CAN_Format_t format = frame->is_extended ? BSP_CAN_FORMAT_EXT_REMOTE : BSP_CAN_FORMAT_STD_REMOTE;
return BSP_CAN_Transmit(can, format, frame->id, NULL, frame->dlc);
}
int8_t BSP_CAN_RegisterId(BSP_CAN_t can, uint32_t can_id, uint8_t queue_size) {
if (!inited) {
return BSP_ERR_INITED;
}
return BSP_CAN_CreateIdQueue(can, can_id, queue_size);
}
int8_t BSP_CAN_UnregisterIdQueue(BSP_CAN_t can, uint32_t can_id) {
if (!inited) {
return BSP_ERR_INITED;
}
return BSP_CAN_DeleteIdQueue(can, can_id);
}
int8_t BSP_CAN_GetMessage(BSP_CAN_t can, uint32_t can_id, BSP_CAN_Message_t *msg, uint32_t timeout) {
if (!inited) {
return BSP_ERR_INITED;
}
if (msg == NULL) {
return BSP_ERR_NULL;
}
if (osMutexAcquire(queue_mutex, CAN_QUEUE_MUTEX_TIMEOUT) != osOK) {
return BSP_ERR_TIMEOUT;
}
osMessageQueueId_t queue = BSP_CAN_FindQueue(can, can_id);
osMutexRelease(queue_mutex);
if (queue == NULL) {
return BSP_ERR_NO_DEV;
}
osStatus_t result = osMessageQueueGet(queue, msg, NULL, timeout);
return (result == osOK) ? BSP_OK : BSP_ERR;
}
int32_t BSP_CAN_GetQueueCount(BSP_CAN_t can, uint32_t can_id) {
if (!inited) {
return -1;
}
if (osMutexAcquire(queue_mutex, CAN_QUEUE_MUTEX_TIMEOUT) != osOK) {
return -1;
}
osMessageQueueId_t queue = BSP_CAN_FindQueue(can, can_id);
osMutexRelease(queue_mutex);
if (queue == NULL) {
return -1;
}
return (int32_t)osMessageQueueGetCount(queue);
}
int8_t BSP_CAN_FlushQueue(BSP_CAN_t can, uint32_t can_id) {
if (!inited) {
return BSP_ERR_INITED;
}
if (osMutexAcquire(queue_mutex, CAN_QUEUE_MUTEX_TIMEOUT) != osOK) {
return BSP_ERR_TIMEOUT;
}
osMessageQueueId_t queue = BSP_CAN_FindQueue(can, can_id);
osMutexRelease(queue_mutex);
if (queue == NULL) {
return BSP_ERR_NO_DEV;
}
BSP_CAN_Message_t temp_msg;
while (osMessageQueueGet(queue, &temp_msg, NULL, BSP_CAN_TIMEOUT_IMMEDIATE) == osOK) {
// 清空
}
return BSP_OK;
}
int8_t BSP_CAN_RegisterIdParser(BSP_CAN_IdParser_t parser) {
if (!inited) {
return BSP_ERR_INITED;
}
if (parser == NULL) {
return BSP_ERR_NULL;
}
id_parser = parser;
return BSP_OK;
}
int8_t BSP_CAN_UnregisterIdParser(void) {
if (!inited) {
return BSP_ERR_INITED;
}
id_parser = BSP_CAN_DefaultIdParser;
return BSP_OK;
}
uint32_t BSP_CAN_ParseId(uint32_t original_id, BSP_CAN_FrameType_t frame_type) {
if (id_parser != NULL) {
return id_parser(original_id, frame_type);
}
return BSP_CAN_DefaultIdParser(original_id, frame_type);
}
/* USER CAN FUNCTIONS BEGIN */
/* USER CAN FUNCTIONS END */

233
User/bsp/can.h
View File

@ -1,233 +0,0 @@
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ----------------------------------------------------------------- */
#include <can.h>
#include "bsp/bsp.h"
#include "bsp/mm.h"
#include <stdint.h>
#include <stdbool.h>
#include <cmsis_os.h>
/* Exported constants ------------------------------------------------------- */
#define BSP_CAN_MAX_DLC 8
#define BSP_CAN_DEFAULT_QUEUE_SIZE 10
#define BSP_CAN_TIMEOUT_IMMEDIATE 0
#define BSP_CAN_TIMEOUT_FOREVER osWaitForever
/* Exported macro ----------------------------------------------------------- */
/* Exported types ----------------------------------------------------------- */
typedef enum {
BSP_CAN_1,
BSP_CAN_2,
BSP_CAN_NUM,
BSP_CAN_ERR,
} BSP_CAN_t;
typedef enum {
HAL_CAN_TX_MAILBOX0_CPLT_CB,
HAL_CAN_TX_MAILBOX1_CPLT_CB,
HAL_CAN_TX_MAILBOX2_CPLT_CB,
HAL_CAN_TX_MAILBOX0_ABORT_CB,
HAL_CAN_TX_MAILBOX1_ABORT_CB,
HAL_CAN_TX_MAILBOX2_ABORT_CB,
HAL_CAN_RX_FIFO0_MSG_PENDING_CB,
HAL_CAN_RX_FIFO0_FULL_CB,
HAL_CAN_RX_FIFO1_MSG_PENDING_CB,
HAL_CAN_RX_FIFO1_FULL_CB,
HAL_CAN_SLEEP_CB,
HAL_CAN_WAKEUP_FROM_RX_MSG_CB,
HAL_CAN_ERROR_CB,
BSP_CAN_CB_NUM,
} BSP_CAN_Callback_t;
/* CAN消息格式枚举 - 用于发送和接收消息时指定格式 */
typedef enum {
BSP_CAN_FORMAT_STD_DATA, /* 标准数据帧 */
BSP_CAN_FORMAT_EXT_DATA, /* 扩展数据帧 */
BSP_CAN_FORMAT_STD_REMOTE, /* 标准远程帧 */
BSP_CAN_FORMAT_EXT_REMOTE, /* 扩展远程帧 */
} BSP_CAN_Format_t;
/* CAN帧类型枚举 - 用于区分不同类型的CAN帧 */
typedef enum {
BSP_CAN_FRAME_STD_DATA, /* 标准数据帧 */
BSP_CAN_FRAME_EXT_DATA, /* 扩展数据帧 */
BSP_CAN_FRAME_STD_REMOTE, /* 标准远程帧 */
BSP_CAN_FRAME_EXT_REMOTE, /* 扩展远程帧 */
} BSP_CAN_FrameType_t;
/* CAN消息结构体 - 支持不同类型帧 */
typedef struct {
BSP_CAN_FrameType_t frame_type; /* 帧类型 */
uint32_t original_id; /* 原始ID（未解析） */
uint32_t parsed_id; /* 解析后的实际ID */
uint8_t dlc; /* 数据长度 */
uint8_t data[BSP_CAN_MAX_DLC]; /* 数据 */
uint32_t timestamp; /* 时间戳（可选） */
} BSP_CAN_Message_t;
/* 标准数据帧结构 */
typedef struct {
uint32_t id; /* CAN ID */
uint8_t dlc; /* 数据长度 */
uint8_t data[BSP_CAN_MAX_DLC]; /* 数据 */
} BSP_CAN_StdDataFrame_t;
/* 扩展数据帧结构 */
typedef struct {
uint32_t id; /* 扩展CAN ID */
uint8_t dlc; /* 数据长度 */
uint8_t data[BSP_CAN_MAX_DLC]; /* 数据 */
} BSP_CAN_ExtDataFrame_t;
/* 远程帧结构 */
typedef struct {
uint32_t id; /* CAN ID */
uint8_t dlc; /* 请求的数据长度 */
bool is_extended; /* 是否为扩展帧 */
} BSP_CAN_RemoteFrame_t;
/* ID解析回调函数类型 */
typedef uint32_t (*BSP_CAN_IdParser_t)(uint32_t original_id, BSP_CAN_FrameType_t frame_type);
/* Exported functions prototypes -------------------------------------------- */
/**
* @brief 初始化 CAN 模块
* @return BSP_OK 成功，其他值失败
*/
int8_t BSP_CAN_Init(void);
/**
* @brief 反初始化 CAN 模块
* @return BSP_OK 成功，其他值失败
*/
int8_t BSP_CAN_DeInit(void);
/**
* @brief 获取 CAN 句柄
* @param can CAN 枚举
* @return CAN_HandleTypeDef 指针，失败返回 NULL
*/
CAN_HandleTypeDef *BSP_CAN_GetHandle(BSP_CAN_t can);
/**
* @brief 注册 CAN 回调函数
* @param can CAN 枚举
* @param type 回调类型
* @param callback 回调函数指针
* @return BSP_OK 成功，其他值失败
*/
int8_t BSP_CAN_RegisterCallback(BSP_CAN_t can, BSP_CAN_Callback_t type,
void (*callback)(void));
/**
* @brief 发送 CAN 消息
* @param can CAN 枚举
* @param format 消息格式
* @param id CAN ID
* @param data 数据指针
* @param dlc 数据长度
* @return BSP_OK 成功，其他值失败
*/
int8_t BSP_CAN_Transmit(BSP_CAN_t can, BSP_CAN_Format_t format,
uint32_t id, uint8_t *data, uint8_t dlc);
/**
* @brief 发送标准数据帧
* @param can CAN 枚举
* @param frame 标准数据帧指针
* @return BSP_OK 成功，其他值失败
*/
int8_t BSP_CAN_TransmitStdDataFrame(BSP_CAN_t can, BSP_CAN_StdDataFrame_t *frame);
/**
* @brief 发送扩展数据帧
* @param can CAN 枚举
* @param frame 扩展数据帧指针
* @return BSP_OK 成功，其他值失败
*/
int8_t BSP_CAN_TransmitExtDataFrame(BSP_CAN_t can, BSP_CAN_ExtDataFrame_t *frame);
/**
* @brief 发送远程帧
* @param can CAN 枚举
* @param frame 远程帧指针
* @return BSP_OK 成功，其他值失败
*/
int8_t BSP_CAN_TransmitRemoteFrame(BSP_CAN_t can, BSP_CAN_RemoteFrame_t *frame);
/**
* @brief 注册 CAN ID 接收队列
* @param can CAN 枚举
* @param can_id 解析后的CAN ID
* @param queue_size 队列大小，0使用默认值
* @return BSP_OK 成功，其他值失败
*/
int8_t BSP_CAN_RegisterId(BSP_CAN_t can, uint32_t can_id, uint8_t queue_size);
/**
* @brief 注销 CAN ID 接收队列
* @param can CAN 枚举
* @param can_id 解析后的CAN ID
* @return BSP_OK 成功，其他值失败
*/
int8_t BSP_CAN_UnregisterIdQueue(BSP_CAN_t can, uint32_t can_id);
/**
* @brief 获取 CAN 消息
* @param can CAN 枚举
* @param can_id 解析后的CAN ID
* @param msg 存储消息的结构体指针
* @param timeout 超时时间（毫秒），0为立即返回，osWaitForever为永久等待
* @return BSP_OK 成功，其他值失败
*/
int8_t BSP_CAN_GetMessage(BSP_CAN_t can, uint32_t can_id, BSP_CAN_Message_t *msg, uint32_t timeout);
/**
* @brief 获取指定ID队列中的消息数量
* @param can CAN 枚举
* @param can_id 解析后的CAN ID
* @return 消息数量，-1表示队列不存在
*/
int32_t BSP_CAN_GetQueueCount(BSP_CAN_t can, uint32_t can_id);
/**
* @brief 清空指定ID队列中的所有消息
* @param can CAN 枚举
* @param can_id 解析后的CAN ID
* @return BSP_OK 成功，其他值失败
*/
int8_t BSP_CAN_FlushQueue(BSP_CAN_t can, uint32_t can_id);
/**
* @brief 注册ID解析器
* @param parser ID解析回调函数
* @return BSP_OK 成功，其他值失败
*/
int8_t BSP_CAN_RegisterIdParser(BSP_CAN_IdParser_t parser);
/**
* @brief 注销ID解析器
* @return BSP_OK 成功，其他值失败
*/
int8_t BSP_CAN_UnregisterIdParser(void);
/**
* @brief 解析CAN ID
* @param original_id 原始ID
* @param frame_type 帧类型
* @return 解析后的ID
*/
uint32_t BSP_CAN_ParseId(uint32_t original_id, BSP_CAN_FrameType_t frame_type);
/* USER CAN FUNCTIONS BEGIN */
/* USER CAN FUNCTIONS END */
#ifdef __cplusplus
}
#endif

122
User/bsp/dwt.c
View File

@ -1,122 +0,0 @@
/**
******************************************************************************
* @file dwt.c
* @author Wang Hongxi
* @version V1.1.0
* @date 2022/3/8
* @brief
******************************************************************************
* @attention
*
******************************************************************************
*/
#include "bsp/dwt.h"
DWT_Time_t SysTime;
static uint32_t CPU_FREQ_Hz, CPU_FREQ_Hz_ms, CPU_FREQ_Hz_us;
static uint32_t CYCCNT_RountCount;
static uint32_t CYCCNT_LAST;
uint64_t CYCCNT64;
static void DWT_CNT_Update(void);
void DWT_Init(uint32_t CPU_Freq_mHz)
{
/* 使能DWT外设 */
CoreDebug->DEMCR |= CoreDebug_DEMCR_TRCENA_Msk;
/* DWT CYCCNT寄存器计数清0 */
DWT->CYCCNT = (uint32_t)0u;
/* 使能Cortex-M DWT CYCCNT寄存器 */
DWT->CTRL |= DWT_CTRL_CYCCNTENA_Msk;
CPU_FREQ_Hz = CPU_Freq_mHz * 1000000;
CPU_FREQ_Hz_ms = CPU_FREQ_Hz / 1000;
CPU_FREQ_Hz_us = CPU_FREQ_Hz / 1000000;
CYCCNT_RountCount = 0;
}
float DWT_GetDeltaT(uint32_t *cnt_last)
{
volatile uint32_t cnt_now = DWT->CYCCNT;
float dt = ((uint32_t)(cnt_now - *cnt_last)) / ((float)(CPU_FREQ_Hz));
*cnt_last = cnt_now;
DWT_CNT_Update();
return dt;
}
double DWT_GetDeltaT64(uint32_t *cnt_last)
{
volatile uint32_t cnt_now = DWT->CYCCNT;
double dt = ((uint32_t)(cnt_now - *cnt_last)) / ((double)(CPU_FREQ_Hz));
*cnt_last = cnt_now;
DWT_CNT_Update();
return dt;
}
void DWT_SysTimeUpdate(void)
{
volatile uint32_t cnt_now = DWT->CYCCNT;
static uint64_t CNT_TEMP1, CNT_TEMP2, CNT_TEMP3;
DWT_CNT_Update();
CYCCNT64 = (uint64_t)CYCCNT_RountCount * (uint64_t)UINT32_MAX + (uint64_t)cnt_now;
CNT_TEMP1 = CYCCNT64 / CPU_FREQ_Hz;
CNT_TEMP2 = CYCCNT64 - CNT_TEMP1 * CPU_FREQ_Hz;
SysTime.s = CNT_TEMP1;
SysTime.ms = CNT_TEMP2 / CPU_FREQ_Hz_ms;
CNT_TEMP3 = CNT_TEMP2 - SysTime.ms * CPU_FREQ_Hz_ms;
SysTime.us = CNT_TEMP3 / CPU_FREQ_Hz_us;
}
float DWT_GetTimeline_s(void)
{
DWT_SysTimeUpdate();
float DWT_Timelinef32 = SysTime.s + SysTime.ms * 0.001f + SysTime.us * 0.000001f;
return DWT_Timelinef32;
}
float DWT_GetTimeline_ms(void)
{
DWT_SysTimeUpdate();
float DWT_Timelinef32 = SysTime.s * 1000 + SysTime.ms + SysTime.us * 0.001f;
return DWT_Timelinef32;
}
uint64_t DWT_GetTimeline_us(void)
{
DWT_SysTimeUpdate();
uint64_t DWT_Timelinef32 = SysTime.s * 1000000 + SysTime.ms * 1000 + SysTime.us;
return DWT_Timelinef32;
}
static void DWT_CNT_Update(void)
{
volatile uint32_t cnt_now = DWT->CYCCNT;
if (cnt_now < CYCCNT_LAST)
CYCCNT_RountCount++;
CYCCNT_LAST = cnt_now;
}
void DWT_Delay(float Delay)
{
uint32_t tickstart = DWT->CYCCNT;
float wait = Delay;
while ((DWT->CYCCNT - tickstart) < wait * (float)CPU_FREQ_Hz)
{
}
}

37
User/bsp/dwt.h
View File

@ -1,37 +0,0 @@
/**
******************************************************************************
* @file dwt.h
* @author Wang Hongxi
* @version V1.1.0
* @date 2022/3/8
* @brief
******************************************************************************
* @attention
*
******************************************************************************
*/
#ifndef _DWT_H
#define _DWT_H
#include "main.h"
#include "stdint.h"
typedef struct
{
uint32_t s;
uint16_t ms;
uint16_t us;
} DWT_Time_t;
void DWT_Init(uint32_t CPU_Freq_mHz);
float DWT_GetDeltaT(uint32_t *cnt_last);
double DWT_GetDeltaT64(uint32_t *cnt_last);
float DWT_GetTimeline_s(void);
float DWT_GetTimeline_ms(void);
uint64_t DWT_GetTimeline_us(void);
void DWT_Delay(float Delay);
void DWT_SysTimeUpdate(void);
extern DWT_Time_t SysTime;
#endif /* DWT_H_ */

114
User/bsp/gpio.c
View File

@ -1,114 +0,0 @@
/* Includes ----------------------------------------------------------------- */
#include "bsp/gpio.h"
#include <gpio.h>
#include <main.h>
/* Private define ----------------------------------------------------------- */
/* Private macro ------------------------------------------------------------ */
/* Private typedef ---------------------------------------------------------- */
typedef struct {
uint16_t pin;
GPIO_TypeDef *gpio;
} BSP_GPIO_MAP_t;
/* Private variables -------------------------------------------------------- */
static const BSP_GPIO_MAP_t GPIO_Map[BSP_GPIO_NUM] = {
{USER_KEY_Pin, USER_KEY_GPIO_Port},
{ACCL_CS_Pin, ACCL_CS_GPIO_Port},
{GYRO_CS_Pin, GYRO_CS_GPIO_Port},
{SPI2_CS_Pin, SPI2_CS_GPIO_Port},
{HW0_Pin, HW0_GPIO_Port},
{HW1_Pin, HW1_GPIO_Port},
{HW2_Pin, HW2_GPIO_Port},
{ACCL_INT_Pin, ACCL_INT_GPIO_Port},
{GYRO_INT_Pin, GYRO_INT_GPIO_Port},
{CMPS_INT_Pin, CMPS_INT_GPIO_Port},
{CMPS_RST_Pin, CMPS_RST_GPIO_Port},
};
static void (*GPIO_Callback[16])(void);
/* Private function -------------------------------------------------------- */
void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin) {
for (uint8_t i = 0; i < 16; i++) {
if (GPIO_Pin & (1 << i)) {
if (GPIO_Callback[i]) {
GPIO_Callback[i]();
}
}
}
}
/* Exported functions ------------------------------------------------------- */
int8_t BSP_GPIO_RegisterCallback(BSP_GPIO_t gpio, void (*callback)(void)) {
if (callback == NULL) return BSP_ERR_NULL;
if (gpio >= BSP_GPIO_NUM) return BSP_ERR;
// 从GPIO映射中获取对应的pin值
uint16_t pin = GPIO_Map[gpio].pin;
for (uint8_t i = 0; i < 16; i++) {
if (pin & (1 << i)) {
GPIO_Callback[i] = callback;
break;
}
}
return BSP_OK;
}
int8_t BSP_GPIO_EnableIRQ(BSP_GPIO_t gpio) {
switch (gpio) {
case BSP_GPIO_USER_KEY:
HAL_NVIC_EnableIRQ(USER_KEY_EXTI_IRQn);
break;
case BSP_GPIO_ACCL_INT:
HAL_NVIC_EnableIRQ(ACCL_INT_EXTI_IRQn);
break;
case BSP_GPIO_GYRO_INT:
HAL_NVIC_EnableIRQ(GYRO_INT_EXTI_IRQn);
break;
case BSP_GPIO_CMPS_INT:
HAL_NVIC_EnableIRQ(CMPS_INT_EXTI_IRQn);
break;
default:
return BSP_ERR;
}
return BSP_OK;
}
int8_t BSP_GPIO_DisableIRQ(BSP_GPIO_t gpio) {
switch (gpio) {
case BSP_GPIO_USER_KEY:
HAL_NVIC_DisableIRQ(USER_KEY_EXTI_IRQn);
break;
case BSP_GPIO_ACCL_INT:
HAL_NVIC_DisableIRQ(ACCL_INT_EXTI_IRQn);
break;
case BSP_GPIO_GYRO_INT:
HAL_NVIC_DisableIRQ(GYRO_INT_EXTI_IRQn);
break;
case BSP_GPIO_CMPS_INT:
HAL_NVIC_DisableIRQ(CMPS_INT_EXTI_IRQn);
break;
default:
return BSP_ERR;
}
return BSP_OK;
}
int8_t BSP_GPIO_WritePin(BSP_GPIO_t gpio, bool value){
if (gpio >= BSP_GPIO_NUM) return BSP_ERR;
HAL_GPIO_WritePin(GPIO_Map[gpio].gpio, GPIO_Map[gpio].pin, value);
return BSP_OK;
}
int8_t BSP_GPIO_TogglePin(BSP_GPIO_t gpio){
if (gpio >= BSP_GPIO_NUM) return BSP_ERR;
HAL_GPIO_TogglePin(GPIO_Map[gpio].gpio, GPIO_Map[gpio].pin);
return BSP_OK;
}
bool BSP_GPIO_ReadPin(BSP_GPIO_t gpio){
if (gpio >= BSP_GPIO_NUM) return false;
return HAL_GPIO_ReadPin(GPIO_Map[gpio].gpio, GPIO_Map[gpio].pin) == GPIO_PIN_SET;
}

45
User/bsp/gpio.h
View File

@ -1,45 +0,0 @@
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ----------------------------------------------------------------- */
#include <stdint.h>
#include <stdbool.h>
#include "bsp/bsp.h"
/* Exported constants ------------------------------------------------------- */
/* Exported macro ----------------------------------------------------------- */
/* Exported types ----------------------------------------------------------- */
typedef enum {
BSP_GPIO_USER_KEY,
BSP_GPIO_ACCL_CS,
BSP_GPIO_GYRO_CS,
BSP_GPIO_SPI2_CS,
BSP_GPIO_HW0,
BSP_GPIO_HW1,
BSP_GPIO_HW2,
BSP_GPIO_ACCL_INT,
BSP_GPIO_GYRO_INT,
BSP_GPIO_CMPS_INT,
BSP_GPIO_CMPS_RST,
BSP_GPIO_NUM,
BSP_GPIO_ERR,
} BSP_GPIO_t;
/* Exported functions prototypes -------------------------------------------- */
int8_t BSP_GPIO_RegisterCallback(BSP_GPIO_t gpio, void (*callback)(void));
int8_t BSP_GPIO_EnableIRQ(BSP_GPIO_t gpio);
int8_t BSP_GPIO_DisableIRQ(BSP_GPIO_t gpio);
int8_t BSP_GPIO_WritePin(BSP_GPIO_t gpio, bool value);
int8_t BSP_GPIO_TogglePin(BSP_GPIO_t gpio);
bool BSP_GPIO_ReadPin(BSP_GPIO_t gpio);
#ifdef __cplusplus
}
#endif

14
User/bsp/mm.c
View File

@ -1,14 +0,0 @@
/* Includes ----------------------------------------------------------------- */
#include "bsp\mm.h"
#include "FreeRTOS.h"
/* Private define ----------------------------------------------------------- */
/* Private macro ------------------------------------------------------------ */
/* Private typedef ---------------------------------------------------------- */
/* Private variables -------------------------------------------------------- */
/* Private function -------------------------------------------------------- */
/* Exported functions ------------------------------------------------------- */
inline void *BSP_Malloc(size_t size) { return pvPortMalloc(size); }
inline void BSP_Free(void *pv) { vPortFree(pv); }

20
User/bsp/mm.h
View File

@ -1,20 +0,0 @@
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ----------------------------------------------------------------- */
#include <stddef.h>
#include <stdint.h>
/* Exported constants ------------------------------------------------------- */
/* Exported macro ----------------------------------------------------------- */
/* Exported types ----------------------------------------------------------- */
/* Exported functions prototypes -------------------------------------------- */
void *BSP_Malloc(size_t size);
void BSP_Free(void *pv);
#ifdef __cplusplus
}
#endif

109
User/bsp/pwm.c
View File

@ -1,109 +0,0 @@
/* Includes ----------------------------------------------------------------- */
#include "tim.h"
#include "bsp/pwm.h"
#include "bsp.h"
/* Private define ----------------------------------------------------------- */
/* Private macro ------------------------------------------------------------ */
/* Private typedef ---------------------------------------------------------- */
typedef struct {
TIM_HandleTypeDef *tim;
uint16_t channel;
} BSP_PWM_Config_t;
/* Private variables -------------------------------------------------------- */
static const BSP_PWM_Config_t PWM_Map[BSP_PWM_NUM] = {
{&htim8, TIM_CHANNEL_1},
{&htim3, TIM_CHANNEL_3},
{&htim4, TIM_CHANNEL_3},
{&htim1, TIM_CHANNEL_2},
{&htim1, TIM_CHANNEL_3},
{&htim1, TIM_CHANNEL_4},
{&htim1, TIM_CHANNEL_1},
{&htim10, TIM_CHANNEL_1},
{&htim5, TIM_CHANNEL_1},
{&htim5, TIM_CHANNEL_2},
{&htim5, TIM_CHANNEL_3},
{&htim8, TIM_CHANNEL_2},
};
/* Private function -------------------------------------------------------- */
/* Exported functions ------------------------------------------------------- */
int8_t BSP_PWM_Start(BSP_PWM_Channel_t ch) {
if (ch >= BSP_PWM_NUM) return BSP_ERR;
HAL_TIM_PWM_Start(PWM_Map[ch].tim, PWM_Map[ch].channel);
return BSP_OK;
}
int8_t BSP_PWM_SetComp(BSP_PWM_Channel_t ch, float duty_cycle) {
if (ch >= BSP_PWM_NUM) return BSP_ERR;
if (duty_cycle > 1.0f) {
duty_cycle = 1.0f;
}
if (duty_cycle < 0.0f) {
duty_cycle = 0.0f;
}
// 获取ARR值（周期值）
uint32_t arr = __HAL_TIM_GET_AUTORELOAD(PWM_Map[ch].tim);
// 计算比较值：CCR = duty_cycle * (ARR + 1)
uint32_t ccr = (uint32_t)(duty_cycle * (arr + 1));
__HAL_TIM_SET_COMPARE(PWM_Map[ch].tim, PWM_Map[ch].channel, ccr);
return BSP_OK;
}
int8_t BSP_PWM_SetFreq(BSP_PWM_Channel_t ch, float freq) {
if (ch >= BSP_PWM_NUM) return BSP_ERR;
uint32_t timer_clock = HAL_RCC_GetPCLK1Freq(); // Get the timer clock frequency
uint32_t prescaler = PWM_Map[ch].tim->Init.Prescaler;
uint32_t period = (timer_clock / (prescaler + 1)) / freq - 1;
if (period > UINT16_MAX) {
return BSP_ERR; // Frequency too low
}
__HAL_TIM_SET_AUTORELOAD(PWM_Map[ch].tim, period);
return BSP_OK;
}
int8_t BSP_PWM_Stop(BSP_PWM_Channel_t ch) {
if (ch >= BSP_PWM_NUM) return BSP_ERR;
HAL_TIM_PWM_Stop(PWM_Map[ch].tim, PWM_Map[ch].channel);
return BSP_OK;
}
uint32_t BSP_PWM_GetAutoReloadPreload(BSP_PWM_Channel_t ch) {
if (ch >= BSP_PWM_NUM) return BSP_ERR;
return PWM_Map[ch].tim->Init.AutoReloadPreload;
}
TIM_HandleTypeDef* BSP_PWM_GetHandle(BSP_PWM_Channel_t ch) {
return PWM_Map[ch].tim;
}
uint16_t BSP_PWM_GetChannel(BSP_PWM_Channel_t ch) {
if (ch >= BSP_PWM_NUM) return BSP_ERR;
return PWM_Map[ch].channel;
}
int8_t BSP_PWM_Start_DMA(BSP_PWM_Channel_t ch, uint32_t *pData, uint16_t Length) {
if (ch >= BSP_PWM_NUM) return BSP_ERR;
HAL_TIM_PWM_Start_DMA(PWM_Map[ch].tim, PWM_Map[ch].channel, pData, Length);
return BSP_OK;
}
int8_t BSP_PWM_Stop_DMA(BSP_PWM_Channel_t ch) {
if (ch >= BSP_PWM_NUM) return BSP_ERR;
HAL_TIM_PWM_Stop_DMA(PWM_Map[ch].tim, PWM_Map[ch].channel);
return BSP_OK;
}

47
User/bsp/pwm.h
View File

@ -1,47 +0,0 @@
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ----------------------------------------------------------------- */
#include <stdint.h>
#include "tim.h"
#include "bsp.h"
/* Exported constants ------------------------------------------------------- */
/* Exported macro ----------------------------------------------------------- */
/* Exported types ----------------------------------------------------------- */
/* PWM通道 */
typedef enum {
BSP_PWM_TIM8_CH1,
BSP_PWM_LASER,
BSP_PWM_BUZZER,
BSP_PWM_TIM1_CH2,
BSP_PWM_TIM1_CH3,
BSP_PWM_TIM1_CH4,
BSP_PWM_TIM1_CH1,
BSP_PWM_IMU_HEAT_PWM,
BSP_PWM_LED_B,
BSP_PWM_LED_G,
BSP_PWM_LED_R,
BSP_PWM_TIM8_CH2,
BSP_PWM_NUM,
BSP_PWM_ERR,
} BSP_PWM_Channel_t;
/* Exported functions prototypes -------------------------------------------- */
int8_t BSP_PWM_Start(BSP_PWM_Channel_t ch);
int8_t BSP_PWM_SetComp(BSP_PWM_Channel_t ch, float duty_cycle);
int8_t BSP_PWM_SetFreq(BSP_PWM_Channel_t ch, float freq);
int8_t BSP_PWM_Stop(BSP_PWM_Channel_t ch);
uint32_t BSP_PWM_GetAutoReloadPreload(BSP_PWM_Channel_t ch);
uint16_t BSP_PWM_GetChannel(BSP_PWM_Channel_t ch);
TIM_HandleTypeDef* BSP_PWM_GetHandle(BSP_PWM_Channel_t ch);
int8_t BSP_PWM_Start_DMA(BSP_PWM_Channel_t ch, uint32_t *pData, uint16_t Length);
int8_t BSP_PWM_Stop_DMA(BSP_PWM_Channel_t ch);
#ifdef __cplusplus
}
#endif

165
User/bsp/spi.c
View File

@ -1,165 +0,0 @@
/* Includes ----------------------------------------------------------------- */
#include <spi.h>
#include "bsp/spi.h"
/* Private define ----------------------------------------------------------- */
/* Private macro ------------------------------------------------------------ */
/* Private typedef ---------------------------------------------------------- */
/* Private variables -------------------------------------------------------- */
static void (*SPI_Callback[BSP_SPI_NUM][BSP_SPI_CB_NUM])(void);
/* Private function -------------------------------------------------------- */
static BSP_SPI_t SPI_Get(SPI_HandleTypeDef *hspi) {
if (hspi->Instance == SPI1)
return BSP_SPI_BMI088;
else
return BSP_SPI_ERR;
}
void HAL_SPI_TxCpltCallback(SPI_HandleTypeDef *hspi) {
BSP_SPI_t bsp_spi = SPI_Get(hspi);
if (bsp_spi != BSP_SPI_ERR) {
if (SPI_Callback[bsp_spi][BSP_SPI_TX_CPLT_CB]) {
SPI_Callback[bsp_spi][BSP_SPI_TX_CPLT_CB]();
}
}
}
void HAL_SPI_RxCpltCallback(SPI_HandleTypeDef *hspi) {
BSP_SPI_t bsp_spi = SPI_Get(hspi);
if (bsp_spi != BSP_SPI_ERR) {
if (SPI_Callback[SPI_Get(hspi)][BSP_SPI_RX_CPLT_CB])
SPI_Callback[SPI_Get(hspi)][BSP_SPI_RX_CPLT_CB]();
}
}
void HAL_SPI_TxRxCpltCallback(SPI_HandleTypeDef *hspi) {
BSP_SPI_t bsp_spi = SPI_Get(hspi);
if (bsp_spi != BSP_SPI_ERR) {
if (SPI_Callback[SPI_Get(hspi)][BSP_SPI_TX_RX_CPLT_CB])
SPI_Callback[SPI_Get(hspi)][BSP_SPI_TX_RX_CPLT_CB]();
}
}
void HAL_SPI_TxHalfCpltCallback(SPI_HandleTypeDef *hspi) {
BSP_SPI_t bsp_spi = SPI_Get(hspi);
if (bsp_spi != BSP_SPI_ERR) {
if (SPI_Callback[SPI_Get(hspi)][BSP_SPI_TX_HALF_CPLT_CB])
SPI_Callback[SPI_Get(hspi)][BSP_SPI_TX_HALF_CPLT_CB]();
}
}
void HAL_SPI_RxHalfCpltCallback(SPI_HandleTypeDef *hspi) {
BSP_SPI_t bsp_spi = SPI_Get(hspi);
if (bsp_spi != BSP_SPI_ERR) {
if (SPI_Callback[SPI_Get(hspi)][BSP_SPI_RX_HALF_CPLT_CB])
SPI_Callback[SPI_Get(hspi)][BSP_SPI_RX_HALF_CPLT_CB]();
}
}
void HAL_SPI_TxRxHalfCpltCallback(SPI_HandleTypeDef *hspi) {
BSP_SPI_t bsp_spi = SPI_Get(hspi);
if (bsp_spi != BSP_SPI_ERR) {
if (SPI_Callback[SPI_Get(hspi)][BSP_SPI_TX_RX_HALF_CPLT_CB])
SPI_Callback[SPI_Get(hspi)][BSP_SPI_TX_RX_HALF_CPLT_CB]();
}
}
void HAL_SPI_ErrorCallback(SPI_HandleTypeDef *hspi) {
BSP_SPI_t bsp_spi = SPI_Get(hspi);
if (bsp_spi != BSP_SPI_ERR) {
if (SPI_Callback[SPI_Get(hspi)][BSP_SPI_ERROR_CB])
SPI_Callback[SPI_Get(hspi)][BSP_SPI_ERROR_CB]();
}
}
void HAL_SPI_AbortCpltCallback(SPI_HandleTypeDef *hspi) {
BSP_SPI_t bsp_spi = SPI_Get(hspi);
if (bsp_spi != BSP_SPI_ERR) {
if (SPI_Callback[SPI_Get(hspi)][BSP_SPI_ABORT_CPLT_CB])
SPI_Callback[SPI_Get(hspi)][BSP_SPI_ABORT_CPLT_CB]();
}
}
/* Exported functions ------------------------------------------------------- */
SPI_HandleTypeDef *BSP_SPI_GetHandle(BSP_SPI_t spi) {
switch (spi) {
case BSP_SPI_BMI088:
return &hspi1;
default:
return NULL;
}
}
int8_t BSP_SPI_RegisterCallback(BSP_SPI_t spi, BSP_SPI_Callback_t type,
void (*callback)(void)) {
if (callback == NULL) return BSP_ERR_NULL;
SPI_Callback[spi][type] = callback;
return BSP_OK;
}
int8_t BSP_SPI_Transmit(BSP_SPI_t spi, uint8_t *data, uint16_t size, bool dma) {
if (spi >= BSP_SPI_NUM) return BSP_ERR;
SPI_HandleTypeDef *hspi = BSP_SPI_GetHandle(spi);
if (hspi == NULL) return BSP_ERR;
if (dma) {
return HAL_SPI_Transmit_DMA(hspi, data, size)!= HAL_OK;;
} else {
return HAL_SPI_Transmit(hspi, data, size, 20)!= HAL_OK;;
}
}
int8_t BSP_SPI_Receive(BSP_SPI_t spi, uint8_t *data, uint16_t size, bool dma) {
if (spi >= BSP_SPI_NUM) return BSP_ERR;
SPI_HandleTypeDef *hspi = BSP_SPI_GetHandle(spi);
if (hspi == NULL) return BSP_ERR;
if (dma) {
return HAL_SPI_Receive_DMA(hspi, data, size)!= HAL_OK;;
} else {
return HAL_SPI_Receive(hspi, data, size, 20)!= HAL_OK;;
}
}
int8_t BSP_SPI_TransmitReceive(BSP_SPI_t spi, uint8_t *txData, uint8_t *rxData,
uint16_t size, bool dma) {
if (spi >= BSP_SPI_NUM) return BSP_ERR;
SPI_HandleTypeDef *hspi = BSP_SPI_GetHandle(spi);
if (hspi == NULL) return BSP_ERR;
if (dma) {
return HAL_SPI_TransmitReceive_DMA(hspi, txData, rxData, size)!= HAL_OK;;
} else {
return HAL_SPI_TransmitReceive(hspi, txData, rxData, size, 20)!= HAL_OK;;
}
}
uint8_t BSP_SPI_MemReadByte(BSP_SPI_t spi, uint8_t reg) {
if (spi >= BSP_SPI_NUM) return 0xFF;
uint8_t tmp[2] = {reg | 0x80, 0x00};
BSP_SPI_TransmitReceive(spi, tmp, tmp, 2u, true);
return tmp[1];
}
int8_t BSP_SPI_MemWriteByte(BSP_SPI_t spi, uint8_t reg, uint8_t data) {
if (spi >= BSP_SPI_NUM) return BSP_ERR;
uint8_t tmp[2] = {reg & 0x7f, data};
return BSP_SPI_Transmit(spi, tmp, 2u, true);
}
int8_t BSP_SPI_MemRead(BSP_SPI_t spi, uint8_t reg, uint8_t *data, uint16_t size) {
if (spi >= BSP_SPI_NUM) return BSP_ERR;
if (data == NULL || size == 0) return BSP_ERR_NULL;
reg = reg | 0x80;
BSP_SPI_Transmit(spi, &reg, 1u, true);
return BSP_SPI_Receive(spi, data, size, true);
}
int8_t BSP_SPI_MemWrite(BSP_SPI_t spi, uint8_t reg, uint8_t *data, uint16_t size) {
if (spi >= BSP_SPI_NUM) return BSP_ERR;
if (data == NULL || size == 0) return BSP_ERR_NULL;
reg = reg & 0x7f;
BSP_SPI_Transmit(spi, &reg, 1u, true);
return BSP_SPI_Transmit(spi, data, size, true);
}

58
User/bsp/spi.h
View File

@ -1,58 +0,0 @@
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ----------------------------------------------------------------- */
#include <spi.h>
#include <stdint.h>
#include <stdbool.h>
#include "bsp/bsp.h"
/* Exported constants ------------------------------------------------------- */
/* Exported macro ----------------------------------------------------------- */
/* Exported types ----------------------------------------------------------- */
/* 要添加使用SPI的新设备，需要先在此添加对应的枚举值 */
/* SPI实体枚举，与设备对应 */
typedef enum {
BSP_SPI_BMI088,
BSP_SPI_NUM,
BSP_SPI_ERR,
} BSP_SPI_t;
/* SPI支持的中断回调函数类型，具体参考HAL中定义 */
typedef enum {
BSP_SPI_TX_CPLT_CB,
BSP_SPI_RX_CPLT_CB,
BSP_SPI_TX_RX_CPLT_CB,
BSP_SPI_TX_HALF_CPLT_CB,
BSP_SPI_RX_HALF_CPLT_CB,
BSP_SPI_TX_RX_HALF_CPLT_CB,
BSP_SPI_ERROR_CB,
BSP_SPI_ABORT_CPLT_CB,
BSP_SPI_CB_NUM,
} BSP_SPI_Callback_t;
/* Exported functions prototypes -------------------------------------------- */
SPI_HandleTypeDef *BSP_SPI_GetHandle(BSP_SPI_t spi);
int8_t BSP_SPI_RegisterCallback(BSP_SPI_t spi, BSP_SPI_Callback_t type,
void (*callback)(void));
int8_t BSP_SPI_Transmit(BSP_SPI_t spi, uint8_t *data, uint16_t size, bool dma);
int8_t BSP_SPI_Receive(BSP_SPI_t spi, uint8_t *data, uint16_t size, bool dma);
int8_t BSP_SPI_TransmitReceive(BSP_SPI_t spi, uint8_t *txData, uint8_t *rxData,
uint16_t size, bool dma);
uint8_t BSP_SPI_MemReadByte(BSP_SPI_t spi, uint8_t reg);
int8_t BSP_SPI_MemWriteByte(BSP_SPI_t spi, uint8_t reg, uint8_t data);
int8_t BSP_SPI_MemRead(BSP_SPI_t spi, uint8_t reg, uint8_t *data, uint16_t size);
int8_t BSP_SPI_MemWrite(BSP_SPI_t spi, uint8_t reg, uint8_t *data, uint16_t size);
#ifdef __cplusplus
}
#endif

65
User/bsp/time.c
View File

@ -1,65 +0,0 @@
/* Includes ----------------------------------------------------------------- */
#include "bsp/time.h"
#include "bsp.h"
#include <cmsis_os2.h>
#include "FreeRTOS.h"
#include "main.h"
#include "task.h"
/* Private define ----------------------------------------------------------- */
/* Private macro ------------------------------------------------------------ */
/* Private typedef ---------------------------------------------------------- */
/* Private variables -------------------------------------------------------- */
/* Private function -------------------------------------------------------- */
/* Exported functions ------------------------------------------------------- */
uint32_t BSP_TIME_Get_ms() { return xTaskGetTickCount(); }
uint64_t BSP_TIME_Get_us() {
uint32_t tick_freq = osKernelGetTickFreq();
uint32_t ticks_old = xTaskGetTickCount()*(1000/tick_freq);
uint32_t tick_value_old = SysTick->VAL;
uint32_t ticks_new = xTaskGetTickCount()*(1000/tick_freq);
uint32_t tick_value_new = SysTick->VAL;
if (ticks_old == ticks_new) {
return ticks_new * 1000 + 1000 - tick_value_old * 1000 / (SysTick->LOAD + 1);
} else {
return ticks_new * 1000 + 1000 - tick_value_new * 1000 / (SysTick->LOAD + 1);
}
}
uint64_t BSP_TIME_Get() __attribute__((alias("BSP_TIME_Get_us")));
int8_t BSP_TIME_Delay_ms(uint32_t ms) {
uint32_t tick_period = 1000u / osKernelGetTickFreq();
uint32_t ticks = ms / tick_period;
switch (osKernelGetState()) {
case osKernelError:
case osKernelReserved:
case osKernelLocked:
case osKernelSuspended:
return BSP_ERR;
case osKernelRunning:
osDelay(ticks ? ticks : 1);
break;
case osKernelInactive:
case osKernelReady:
HAL_Delay(ms);
break;
}
return BSP_OK;
}
/*阻塞us延迟*/
int8_t BSP_TIME_Delay_us(uint32_t us) {
uint64_t start = BSP_TIME_Get_us();
while (BSP_TIME_Get_us() - start < us) {
// 等待us时间
}
return BSP_OK;
}
int8_t BSP_TIME_Delay(uint32_t ms) __attribute__((alias("BSP_TIME_Delay_ms")));

31
User/bsp/time.h
View File

@ -1,31 +0,0 @@
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ----------------------------------------------------------------- */
#include <stdint.h>
#include "bsp/bsp.h"
/* Exported constants ------------------------------------------------------- */
/* Exported macro ----------------------------------------------------------- */
/* Exported types ----------------------------------------------------------- */
/* Exported functions prototypes -------------------------------------------- */
uint32_t BSP_TIME_Get_ms();
uint64_t BSP_TIME_Get_us();
uint64_t BSP_TIME_Get();
int8_t BSP_TIME_Delay_ms(uint32_t ms);
/*微秒阻塞延时，一般别用*/
int8_t BSP_TIME_Delay_us(uint32_t us);
int8_t BSP_TIME_Delay(uint32_t ms);
#ifdef __cplusplus
}
#endif

139
User/bsp/uart.c
View File

@ -1,139 +0,0 @@
/* Includes ----------------------------------------------------------------- */
#include <usart.h>
#include "bsp/uart.h"
/* Private define ----------------------------------------------------------- */
/* Private macro ------------------------------------------------------------ */
/* Private typedef ---------------------------------------------------------- */
/* Private variables -------------------------------------------------------- */
static void (*UART_Callback[BSP_UART_NUM][BSP_UART_CB_NUM])(void);
/* Private function -------------------------------------------------------- */
static BSP_UART_t UART_Get(UART_HandleTypeDef *huart) {
if (huart->Instance == USART3)
return BSP_UART_DR16;
else
return BSP_UART_ERR;
}
void HAL_UART_TxCpltCallback(UART_HandleTypeDef *huart) {
BSP_UART_t bsp_uart = UART_Get(huart);
if (bsp_uart != BSP_UART_ERR) {
if (UART_Callback[bsp_uart][BSP_UART_TX_CPLT_CB]) {
UART_Callback[bsp_uart][BSP_UART_TX_CPLT_CB]();
}
}
}
void HAL_UART_TxHalfCpltCallback(UART_HandleTypeDef *huart) {
BSP_UART_t bsp_uart = UART_Get(huart);
if (bsp_uart != BSP_UART_ERR) {
if (UART_Callback[bsp_uart][BSP_UART_TX_HALF_CPLT_CB]) {
UART_Callback[bsp_uart][BSP_UART_TX_HALF_CPLT_CB]();
}
}
}
void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart) {
BSP_UART_t bsp_uart = UART_Get(huart);
if (bsp_uart != BSP_UART_ERR) {
if (UART_Callback[bsp_uart][BSP_UART_RX_CPLT_CB]) {
UART_Callback[bsp_uart][BSP_UART_RX_CPLT_CB]();
}
}
}
void HAL_UART_RxHalfCpltCallback(UART_HandleTypeDef *huart) {
BSP_UART_t bsp_uart = UART_Get(huart);
if (bsp_uart != BSP_UART_ERR) {
if (UART_Callback[bsp_uart][BSP_UART_RX_HALF_CPLT_CB]) {
UART_Callback[bsp_uart][BSP_UART_RX_HALF_CPLT_CB]();
}
}
}
void HAL_UART_ErrorCallback(UART_HandleTypeDef *huart) {
BSP_UART_t bsp_uart = UART_Get(huart);
if (bsp_uart != BSP_UART_ERR) {
if (UART_Callback[bsp_uart][BSP_UART_ERROR_CB]) {
UART_Callback[bsp_uart][BSP_UART_ERROR_CB]();
}
}
}
void HAL_UART_AbortCpltCallback(UART_HandleTypeDef *huart) {
BSP_UART_t bsp_uart = UART_Get(huart);
if (bsp_uart != BSP_UART_ERR) {
if (UART_Callback[bsp_uart][BSP_UART_ABORT_CPLT_CB]) {
UART_Callback[bsp_uart][BSP_UART_ABORT_CPLT_CB]();
}
}
}
void HAL_UART_AbortTransmitCpltCallback(UART_HandleTypeDef *huart) {
BSP_UART_t bsp_uart = UART_Get(huart);
if (bsp_uart != BSP_UART_ERR) {
if (UART_Callback[bsp_uart][BSP_UART_ABORT_TX_CPLT_CB]) {
UART_Callback[bsp_uart][BSP_UART_ABORT_TX_CPLT_CB]();
}
}
}
void HAL_UART_AbortReceiveCpltCallback(UART_HandleTypeDef *huart) {
BSP_UART_t bsp_uart = UART_Get(huart);
if (bsp_uart != BSP_UART_ERR) {
if (UART_Callback[bsp_uart][BSP_UART_ABORT_RX_CPLT_CB]) {
UART_Callback[bsp_uart][BSP_UART_ABORT_RX_CPLT_CB]();
}
}
}
/* Exported functions ------------------------------------------------------- */
void BSP_UART_IRQHandler(UART_HandleTypeDef *huart) {
if (__HAL_UART_GET_FLAG(huart, UART_FLAG_IDLE)) {
__HAL_UART_CLEAR_IDLEFLAG(huart);
if (UART_Callback[UART_Get(huart)][BSP_UART_IDLE_LINE_CB]) {
UART_Callback[UART_Get(huart)][BSP_UART_IDLE_LINE_CB]();
}
}
}
UART_HandleTypeDef *BSP_UART_GetHandle(BSP_UART_t uart) {
switch (uart) {
case BSP_UART_DR16:
return &huart3;
default:
return NULL;
}
}
int8_t BSP_UART_RegisterCallback(BSP_UART_t uart, BSP_UART_Callback_t type,
void (*callback)(void)) {
if (callback == NULL) return BSP_ERR_NULL;
if (uart >= BSP_UART_NUM || type >= BSP_UART_CB_NUM) return BSP_ERR;
UART_Callback[uart][type] = callback;
return BSP_OK;
}
int8_t BSP_UART_Transmit(BSP_UART_t uart, uint8_t *data, uint16_t size, bool dma) {
if (uart >= BSP_UART_NUM) return BSP_ERR;
if (data == NULL || size == 0) return BSP_ERR_NULL;
if (dma) {
return HAL_UART_Transmit_DMA(BSP_UART_GetHandle(uart), data, size);
} else {
return HAL_UART_Transmit_IT(BSP_UART_GetHandle(uart), data, size);
}
}
int8_t BSP_UART_Receive(BSP_UART_t uart, uint8_t *data, uint16_t size, bool dma) {
if (uart >= BSP_UART_NUM) return BSP_ERR;
if (data == NULL || size == 0) return BSP_ERR_NULL;
if (dma) {
return HAL_UART_Receive_DMA(BSP_UART_GetHandle(uart), data, size);
} else {
return HAL_UART_Receive_IT(BSP_UART_GetHandle(uart), data, size);
}
}

53
User/bsp/uart.h
View File

@ -1,53 +0,0 @@
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ----------------------------------------------------------------- */
#include <usart.h>
#include <stdint.h>
#include <stdbool.h>
#include "bsp/bsp.h"
/* Exported constants ------------------------------------------------------- */
/* Exported macro ----------------------------------------------------------- */
/* Exported types ----------------------------------------------------------- */
/* 要添加使用UART的新设备，需要先在此添加对应的枚举值 */
/* UART实体枚举，与设备对应 */
typedef enum {
BSP_UART_DR16,
BSP_UART_NUM,
BSP_UART_ERR,
} BSP_UART_t;
/* UART支持的中断回调函数类型，具体参考HAL中定义 */
typedef enum {
BSP_UART_TX_HALF_CPLT_CB,
BSP_UART_TX_CPLT_CB,
BSP_UART_RX_HALF_CPLT_CB,
BSP_UART_RX_CPLT_CB,
BSP_UART_ERROR_CB,
BSP_UART_ABORT_CPLT_CB,
BSP_UART_ABORT_TX_CPLT_CB,
BSP_UART_ABORT_RX_CPLT_CB,
BSP_UART_IDLE_LINE_CB,
BSP_UART_CB_NUM,
} BSP_UART_Callback_t;
/* Exported functions prototypes -------------------------------------------- */
UART_HandleTypeDef *BSP_UART_GetHandle(BSP_UART_t uart);
int8_t BSP_UART_RegisterCallback(BSP_UART_t uart, BSP_UART_Callback_t type,
void (*callback)(void));
int8_t BSP_UART_Transmit(BSP_UART_t uart, uint8_t *data, uint16_t size, bool dma);
int8_t BSP_UART_Receive(BSP_UART_t uart, uint8_t *data, uint16_t size, bool dma);
#ifdef __cplusplus
}
#endif

405
User/component/ahrs.c
View File

@ -1,405 +0,0 @@
/*
开源的AHRS算法。
MadgwickAHRS
*/
#include "ahrs.h"
#include <string.h>
#include "user_math.h"
#define BETA_IMU (0.033f)
#define BETA_AHRS (0.041f)
/* 2 * proportional gain (Kp) */
static float beta = BETA_IMU;
/**
* @brief 不使用磁力计计算姿态
*
* @param ahrs 姿态解算主结构体
* @param accl 加速度计数据
* @param gyro 陀螺仪数据
* @return int8_t 0对应没有错误
*/
static int8_t AHRS_UpdateIMU(AHRS_t *ahrs, const AHRS_Accl_t *accl,
const AHRS_Gyro_t *gyro) {
if (ahrs == NULL) return -1;
if (accl == NULL) return -1;
if (gyro == NULL) return -1;
beta = BETA_IMU;
float ax = accl->x;
float ay = accl->y;
float az = accl->z;
float gx = gyro->x;
float gy = gyro->y;
float gz = gyro->z;
float recip_norm;
float s0, s1, s2, s3;
float q_dot1, q_dot2, q_dot3, q_dot4;
float _2q0, _2q1, _2q2, _2q3, _4q0, _4q1, _4q2, _8q1, _8q2, q0q0, q1q1, q2q2,
q3q3;
/* Rate of change of quaternion from gyroscope */
q_dot1 = 0.5f * (-ahrs->quat.q1 * gx - ahrs->quat.q2 * gy -
ahrs->quat.q3 * gz);
q_dot2 = 0.5f * (ahrs->quat.q0 * gx + ahrs->quat.q2 * gz -
ahrs->quat.q3 * gy);
q_dot3 = 0.5f * (ahrs->quat.q0 * gy - ahrs->quat.q1 * gz +
ahrs->quat.q3 * gx);
q_dot4 = 0.5f * (ahrs->quat.q0 * gz + ahrs->quat.q1 * gy -
ahrs->quat.q2 * gx);
/* Compute feedback only if accelerometer measurement valid (avoids NaN in
* accelerometer normalisation) */
if (!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
/* Normalise accelerometer measurement */
recip_norm = InvSqrt(ax * ax + ay * ay + az * az);
ax *= recip_norm;
ay *= recip_norm;
az *= recip_norm;
/* Auxiliary variables to avoid repeated arithmetic */
_2q0 = 2.0f * ahrs->quat.q0;
_2q1 = 2.0f * ahrs->quat.q1;
_2q2 = 2.0f * ahrs->quat.q2;
_2q3 = 2.0f * ahrs->quat.q3;
_4q0 = 4.0f * ahrs->quat.q0;
_4q1 = 4.0f * ahrs->quat.q1;
_4q2 = 4.0f * ahrs->quat.q2;
_8q1 = 8.0f * ahrs->quat.q1;
_8q2 = 8.0f * ahrs->quat.q2;
q0q0 = ahrs->quat.q0 * ahrs->quat.q0;
q1q1 = ahrs->quat.q1 * ahrs->quat.q1;
q2q2 = ahrs->quat.q2 * ahrs->quat.q2;
q3q3 = ahrs->quat.q3 * ahrs->quat.q3;
/* Gradient decent algorithm corrective step */
s0 = _4q0 * q2q2 + _2q2 * ax + _4q0 * q1q1 - _2q1 * ay;
s1 = _4q1 * q3q3 - _2q3 * ax + 4.0f * q0q0 * ahrs->quat.q1 -
_2q0 * ay - _4q1 + _8q1 * q1q1 + _8q1 * q2q2 + _4q1 * az;
s2 = 4.0f * q0q0 * ahrs->quat.q2 + _2q0 * ax + _4q2 * q3q3 -
_2q3 * ay - _4q2 + _8q2 * q1q1 + _8q2 * q2q2 + _4q2 * az;
s3 = 4.0f * q1q1 * ahrs->quat.q3 - _2q1 * ax +
4.0f * q2q2 * ahrs->quat.q3 - _2q2 * ay;
/* normalise step magnitude */
recip_norm = InvSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3);
s0 *= recip_norm;
s1 *= recip_norm;
s2 *= recip_norm;
s3 *= recip_norm;
/* Apply feedback step */
q_dot1 -= beta * s0;
q_dot2 -= beta * s1;
q_dot3 -= beta * s2;
q_dot4 -= beta * s3;
}
/* Integrate rate of change of quaternion to yield quaternion */
ahrs->quat.q0 += q_dot1 * ahrs->inv_sample_freq;
ahrs->quat.q1 += q_dot2 * ahrs->inv_sample_freq;
ahrs->quat.q2 += q_dot3 * ahrs->inv_sample_freq;
ahrs->quat.q3 += q_dot4 * ahrs->inv_sample_freq;
/* Normalise quaternion */
recip_norm = InvSqrt(ahrs->quat.q0 * ahrs->quat.q0 +
ahrs->quat.q1 * ahrs->quat.q1 +
ahrs->quat.q2 * ahrs->quat.q2 +
ahrs->quat.q3 * ahrs->quat.q3);
ahrs->quat.q0 *= recip_norm;
ahrs->quat.q1 *= recip_norm;
ahrs->quat.q2 *= recip_norm;
ahrs->quat.q3 *= recip_norm;
return 0;
}
/**
* @brief 初始化姿态解算
*
* @param ahrs 姿态解算主结构体
* @param magn 磁力计数据
* @param sample_freq 采样频率
* @return int8_t 0对应没有错误
*/
int8_t AHRS_Init(AHRS_t *ahrs, const AHRS_Magn_t *magn, float sample_freq) {
if (ahrs == NULL) return -1;
ahrs->inv_sample_freq = 1.0f / sample_freq;
ahrs->quat.q0 = 1.0f;
ahrs->quat.q1 = 0.0f;
ahrs->quat.q2 = 0.0f;
ahrs->quat.q3 = 0.0f;
if (magn) {
float yaw = -atan2(magn->y, magn->x);
if ((magn->x == 0.0f) && (magn->y == 0.0f) && (magn->z == 0.0f)) {
ahrs->quat.q0 = 0.800884545f;
ahrs->quat.q1 = 0.00862364192f;
ahrs->quat.q2 = -0.00283267116f;
ahrs->quat.q3 = 0.598749936f;
} else if ((yaw < (M_PI / 2.0f)) || (yaw > 0.0f)) {
ahrs->quat.q0 = 0.997458339f;
ahrs->quat.q1 = 0.000336312107f;
ahrs->quat.q2 = -0.0057230792f;
ahrs->quat.q3 = 0.0740156546;
} else if ((yaw < M_PI) || (yaw > (M_PI / 2.0f))) {
ahrs->quat.q0 = 0.800884545f;
ahrs->quat.q1 = 0.00862364192f;
ahrs->quat.q2 = -0.00283267116f;
ahrs->quat.q3 = 0.598749936f;
} else if ((yaw < 90.0f) || (yaw > M_PI)) {
ahrs->quat.q0 = 0.800884545f;
ahrs->quat.q1 = 0.00862364192f;
ahrs->quat.q2 = -0.00283267116f;
ahrs->quat.q3 = 0.598749936f;
} else if ((yaw < 90.0f) || (yaw > 0.0f)) {
ahrs->quat.q0 = 0.800884545f;
ahrs->quat.q1 = 0.00862364192f;
ahrs->quat.q2 = -0.00283267116f;
ahrs->quat.q3 = 0.598749936f;
}
}
return 0;
}
/**
* @brief 姿态运算更新一次
* @note 输入数据必须是NED(North East Down) 参考坐标系
*
* @param ahrs 姿态解算主结构体
* @param accl 加速度计数据
* @param gyro 陀螺仪数据
* @param magn 磁力计数据
* @return int8_t 0对应没有错误
*/
int8_t AHRS_Update(AHRS_t *ahrs, const AHRS_Accl_t *accl,
const AHRS_Gyro_t *gyro, const AHRS_Magn_t *magn) {
if (ahrs == NULL) return -1;
if (accl == NULL) return -1;
if (gyro == NULL) return -1;
beta = BETA_AHRS;
float recip_norm;
float s0, s1, s2, s3;
float q_dot1, q_dot2, q_dot3, q_dot4;
float hx, hy;
float _2q0mx, _2q0my, _2q0mz, _2q1mx, _2bx, _2bz, _4bx, _4bz, _2q0, _2q1,
_2q2, _2q3, _2q0q2, _2q2q3, q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3,
q2q2, q2q3, q3q3;
if (magn == NULL) return AHRS_UpdateIMU(ahrs, accl, gyro);
float mx = magn->x;
float my = magn->y;
float mz = magn->z;
/* Use IMU algorithm if magnetometer measurement invalid (avoids NaN in */
/* magnetometer normalisation) */
if ((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) {
return AHRS_UpdateIMU(ahrs, accl, gyro);
}
float ax = accl->x;
float ay = accl->y;
float az = accl->z;
float gx = gyro->x;
float gy = gyro->y;
float gz = gyro->z;
/* Rate of change of quaternion from gyroscope */
q_dot1 = 0.5f * (-ahrs->quat.q1 * gx - ahrs->quat.q2 * gy -
ahrs->quat.q3 * gz);
q_dot2 = 0.5f * (ahrs->quat.q0 * gx + ahrs->quat.q2 * gz -
ahrs->quat.q3 * gy);
q_dot3 = 0.5f * (ahrs->quat.q0 * gy - ahrs->quat.q1 * gz +
ahrs->quat.q3 * gx);
q_dot4 = 0.5f * (ahrs->quat.q0 * gz + ahrs->quat.q1 * gy -
ahrs->quat.q2 * gx);
/* Compute feedback only if accelerometer measurement valid (avoids NaN in
* accelerometer normalisation) */
if (!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
/* Normalise accelerometer measurement */
recip_norm = InvSqrt(ax * ax + ay * ay + az * az);
ax *= recip_norm;
ay *= recip_norm;
az *= recip_norm;
/* Normalise magnetometer measurement */
recip_norm = InvSqrt(mx * mx + my * my + mz * mz);
mx *= recip_norm;
my *= recip_norm;
mz *= recip_norm;
/* Auxiliary variables to avoid repeated arithmetic */
_2q0mx = 2.0f * ahrs->quat.q0 * mx;
_2q0my = 2.0f * ahrs->quat.q0 * my;
_2q0mz = 2.0f * ahrs->quat.q0 * mz;
_2q1mx = 2.0f * ahrs->quat.q1 * mx;
_2q0 = 2.0f * ahrs->quat.q0;
_2q1 = 2.0f * ahrs->quat.q1;
_2q2 = 2.0f * ahrs->quat.q2;
_2q3 = 2.0f * ahrs->quat.q3;
_2q0q2 = 2.0f * ahrs->quat.q0 * ahrs->quat.q2;
_2q2q3 = 2.0f * ahrs->quat.q2 * ahrs->quat.q3;
q0q0 = ahrs->quat.q0 * ahrs->quat.q0;
q0q1 = ahrs->quat.q0 * ahrs->quat.q1;
q0q2 = ahrs->quat.q0 * ahrs->quat.q2;
q0q3 = ahrs->quat.q0 * ahrs->quat.q3;
q1q1 = ahrs->quat.q1 * ahrs->quat.q1;
q1q2 = ahrs->quat.q1 * ahrs->quat.q2;
q1q3 = ahrs->quat.q1 * ahrs->quat.q3;
q2q2 = ahrs->quat.q2 * ahrs->quat.q2;
q2q3 = ahrs->quat.q2 * ahrs->quat.q3;
q3q3 = ahrs->quat.q3 * ahrs->quat.q3;
/* Reference direction of Earth's magnetic field */
hx = mx * q0q0 - _2q0my * ahrs->quat.q3 +
_2q0mz * ahrs->quat.q2 + mx * q1q1 +
_2q1 * my * ahrs->quat.q2 + _2q1 * mz * ahrs->quat.q3 -
mx * q2q2 - mx * q3q3;
hy = _2q0mx * ahrs->quat.q3 + my * q0q0 -
_2q0mz * ahrs->quat.q1 + _2q1mx * ahrs->quat.q2 -
my * q1q1 + my * q2q2 + _2q2 * mz * ahrs->quat.q3 - my * q3q3;
// _2bx = sqrtf(hx * hx + hy * hy);
// 改为invsqrt
_2bx = 1.f / InvSqrt(hx * hx + hy * hy);
_2bz = -_2q0mx * ahrs->quat.q2 + _2q0my * ahrs->quat.q1 +
mz * q0q0 + _2q1mx * ahrs->quat.q3 - mz * q1q1 +
_2q2 * my * ahrs->quat.q3 - mz * q2q2 + mz * q3q3;
_4bx = 2.0f * _2bx;
_4bz = 2.0f * _2bz;
/* Gradient decent algorithm corrective step */
s0 = -_2q2 * (2.0f * q1q3 - _2q0q2 - ax) +
_2q1 * (2.0f * q0q1 + _2q2q3 - ay) -
_2bz * ahrs->quat.q2 *
(_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) +
(-_2bx * ahrs->quat.q3 + _2bz * ahrs->quat.q1) *
(_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) +
_2bx * ahrs->quat.q2 *
(_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
s1 = _2q3 * (2.0f * q1q3 - _2q0q2 - ax) +
_2q0 * (2.0f * q0q1 + _2q2q3 - ay) -
4.0f * ahrs->quat.q1 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) +
_2bz * ahrs->quat.q3 *
(_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) +
(_2bx * ahrs->quat.q2 + _2bz * ahrs->quat.q0) *
(_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) +
(_2bx * ahrs->quat.q3 - _4bz * ahrs->quat.q1) *
(_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
s2 = -_2q0 * (2.0f * q1q3 - _2q0q2 - ax) +
_2q3 * (2.0f * q0q1 + _2q2q3 - ay) -
4.0f * ahrs->quat.q2 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) +
(-_4bx * ahrs->quat.q2 - _2bz * ahrs->quat.q0) *
(_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) +
(_2bx * ahrs->quat.q1 + _2bz * ahrs->quat.q3) *
(_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) +
(_2bx * ahrs->quat.q0 - _4bz * ahrs->quat.q2) *
(_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
s3 = _2q1 * (2.0f * q1q3 - _2q0q2 - ax) +
_2q2 * (2.0f * q0q1 + _2q2q3 - ay) +
(-_4bx * ahrs->quat.q3 + _2bz * ahrs->quat.q1) *
(_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) +
(-_2bx * ahrs->quat.q0 + _2bz * ahrs->quat.q2) *
(_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) +
_2bx * ahrs->quat.q1 *
(_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
/* normalise step magnitude */
recip_norm = InvSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3);
s0 *= recip_norm;
s1 *= recip_norm;
s2 *= recip_norm;
s3 *= recip_norm;
/* Apply feedback step */
q_dot1 -= beta * s0;
q_dot2 -= beta * s1;
q_dot3 -= beta * s2;
q_dot4 -= beta * s3;
}
/* Integrate rate of change of quaternion to yield quaternion */
ahrs->quat.q0 += q_dot1 * ahrs->inv_sample_freq;
ahrs->quat.q1 += q_dot2 * ahrs->inv_sample_freq;
ahrs->quat.q2 += q_dot3 * ahrs->inv_sample_freq;
ahrs->quat.q3 += q_dot4 * ahrs->inv_sample_freq;
/* Normalise quaternion */
recip_norm = InvSqrt(ahrs->quat.q0 * ahrs->quat.q0 +
ahrs->quat.q1 * ahrs->quat.q1 +
ahrs->quat.q2 * ahrs->quat.q2 +
ahrs->quat.q3 * ahrs->quat.q3);
ahrs->quat.q0 *= recip_norm;
ahrs->quat.q1 *= recip_norm;
ahrs->quat.q2 *= recip_norm;
ahrs->quat.q3 *= recip_norm;
return 0;
}
/**
* @brief 通过姿态解算主结构体中的四元数计算欧拉角
*
* @param eulr 欧拉角
* @param ahrs 姿态解算主结构体
* @return int8_t 0对应没有错误
*/
int8_t AHRS_GetEulr(AHRS_Eulr_t *eulr, const AHRS_t *ahrs) {
if (eulr == NULL) return -1;
if (ahrs == NULL) return -1;
const float sinr_cosp = 2.0f * (ahrs->quat.q0 * ahrs->quat.q1 +
ahrs->quat.q2 * ahrs->quat.q3);
const float cosr_cosp =
1.0f - 2.0f * (ahrs->quat.q1 * ahrs->quat.q1 +
ahrs->quat.q2 * ahrs->quat.q2);
eulr->pit = atan2f(sinr_cosp, cosr_cosp);
const float sinp = 2.0f * (ahrs->quat.q0 * ahrs->quat.q2 -
ahrs->quat.q3 * ahrs->quat.q1);
if (fabsf(sinp) >= 1.0f)
eulr->rol = copysignf(M_PI / 2.0f, sinp);
else
eulr->rol = asinf(sinp);
const float siny_cosp = 2.0f * (ahrs->quat.q0 * ahrs->quat.q3 +
ahrs->quat.q1 * ahrs->quat.q2);
const float cosy_cosp =
1.0f - 2.0f * (ahrs->quat.q2 * ahrs->quat.q2 +
ahrs->quat.q3 * ahrs->quat.q3);
eulr->yaw = atan2f(siny_cosp, cosy_cosp);
#if 0
eulr->yaw *= M_RAD2DEG_MULT;
eulr->rol *= M_RAD2DEG_MULT;
eulr->pit *= M_RAD2DEG_MULT;
#endif
return 0;
}
/**
* \brief 将对应数据置零
*
* \param eulr 被操作的数据
*/
void AHRS_ResetEulr(AHRS_Eulr_t *eulr) { memset(eulr, 0, sizeof(*eulr)); }

98
User/component/ahrs.h
View File

@ -1,98 +0,0 @@
/*
开源的AHRS算法。
MadgwickAHRS
*/
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
#include "user_math.h"
/* 欧拉角（Euler angle） */
typedef struct {
float yaw; /* 偏航角（Yaw angle） */
float pit; /* 俯仰角（Pitch angle） */
float rol; /* 翻滚角（Roll angle） */
} AHRS_Eulr_t;
/* 加速度计 Accelerometer */
typedef struct {
float x;
float y;
float z;
} AHRS_Accl_t;
/* 陀螺仪 Gyroscope */
typedef struct {
float x;
float y;
float z;
} AHRS_Gyro_t;
/* 磁力计 Magnetometer */
typedef struct {
float x;
float y;
float z;
} AHRS_Magn_t;
/* 四元数 */
typedef struct {
float q0;
float q1;
float q2;
float q3;
} AHRS_Quaternion_t;
/* 姿态解算算法主结构体 */
typedef struct {
/* 四元数 */
AHRS_Quaternion_t quat;
float inv_sample_freq; /* 采样频率的的倒数 */
} AHRS_t;
/**
* @brief 初始化姿态解算
*
* @param ahrs 姿态解算主结构体
* @param magn 磁力计数据
* @param sample_freq 采样频率
* @return int8_t 0对应没有错误
*/
int8_t AHRS_Init(AHRS_t *ahrs, const AHRS_Magn_t *magn, float sample_freq);
/**
* @brief 姿态运算更新一次
*
* @param ahrs 姿态解算主结构体
* @param accl 加速度计数据
* @param gyro 陀螺仪数据
* @param magn 磁力计数据
* @return int8_t 0对应没有错误
*/
int8_t AHRS_Update(AHRS_t *ahrs, const AHRS_Accl_t *accl,
const AHRS_Gyro_t *gyro, const AHRS_Magn_t *magn);
/**
* @brief 通过姿态解算主结构体中的四元数计算欧拉角
*
* @param eulr 欧拉角
* @param ahrs 姿态解算主结构体
* @return int8_t 0对应没有错误
*/
int8_t AHRS_GetEulr(AHRS_Eulr_t *eulr, const AHRS_t *ahrs);
/**
* \brief 将对应数据置零
*
* \param eulr 被操作的数据
*/
void AHRS_ResetEulr(AHRS_Eulr_t *eulr);
#ifdef __cplusplus
}
#endif

375
User/component/cmd.c
View File

@ -1,375 +0,0 @@
/*
控制命令
*/
#include "cmd.h"
#include <string.h>
/**
* @brief 行为转换为对应按键
*
* @param cmd 主结构体
* @param behavior 行为
* @return uint16_t 行为对应的按键
*/
static inline CMD_KeyValue_t CMD_BehaviorToKey(CMD_t *cmd,
CMD_Behavior_t behavior) {
return cmd->param->map.key_map[behavior].key;
}
static inline CMD_ActiveType_t CMD_BehaviorToActive(CMD_t *cmd,
CMD_Behavior_t behavior) {
return cmd->param->map.key_map[behavior].active;
}
/**
* @brief 检查按键是否按下
*
* @param rc 遥控器数据
* @param key 按键名称
* @param stateful 是否为状态切换按键
* @return true 按下
* @return false 未按下
*/
static bool CMD_KeyPressedRc(const CMD_RC_t *rc, CMD_KeyValue_t key) {
/* 按下按键为鼠标左、右键 */
if (key == CMD_L_CLICK) {
return rc->mouse.l_click;
}
if (key == CMD_R_CLICK) {
return rc->mouse.r_click;
}
return rc->key & (1u << key);
}
static bool CMD_BehaviorOccurredRc(const CMD_RC_t *rc, CMD_t *cmd,
CMD_Behavior_t behavior) {
CMD_KeyValue_t key = CMD_BehaviorToKey(cmd, behavior);
CMD_ActiveType_t active = CMD_BehaviorToActive(cmd, behavior);
bool now_key_pressed, last_key_pressed;
/* 按下按键为鼠标左、右键 */
if (key == CMD_L_CLICK) {
now_key_pressed = rc->mouse.l_click;
last_key_pressed = cmd->mouse_last.l_click;
} else if (key == CMD_R_CLICK) {
now_key_pressed = rc->mouse.r_click;
last_key_pressed = cmd->mouse_last.r_click;
} else {
now_key_pressed = rc->key & (1u << key);
last_key_pressed = cmd->key_last & (1u << key);
}
switch (active) {
case CMD_ACTIVE_PRESSING:
return now_key_pressed && !last_key_pressed;
case CMD_ACTIVE_RASING:
return !now_key_pressed && last_key_pressed;
case CMD_ACTIVE_PRESSED:
return now_key_pressed;
}
}
/**
* @brief 解析pc行为逻辑
*
* @param rc 遥控器数据
* @param cmd 主结构体
* @param dt_sec 两次解析的间隔
*/
static void CMD_PcLogic(const CMD_RC_t *rc, CMD_t *cmd, float dt_sec) {
cmd->gimbal.mode = GIMBAL_MODE_ABSOLUTE;
/* 云台设置为鼠标控制欧拉角的变化，底盘的控制向量设置为零 */
cmd->gimbal.delta_eulr.yaw =
(float)rc->mouse.x * dt_sec * cmd->param->sens_mouse;
cmd->gimbal.delta_eulr.pit =
(float)(-rc->mouse.y) * dt_sec * cmd->param->sens_mouse;
cmd->chassis.ctrl_vec.vx = cmd->chassis.ctrl_vec.vy = 0.0f;
cmd->shoot.reverse_trig = false;
/* 按键行为映射相关逻辑 */
if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_FORE)) {
cmd->chassis.ctrl_vec.vy += cmd->param->move.move_sense;
}
if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_BACK)) {
cmd->chassis.ctrl_vec.vy -= cmd->param->move.move_sense;
}
if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_LEFT)) {
cmd->chassis.ctrl_vec.vx -= cmd->param->move.move_sense;
}
if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_RIGHT)) {
cmd->chassis.ctrl_vec.vx += cmd->param->move.move_sense;
}
if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_ACCELERATE)) {
cmd->chassis.ctrl_vec.vx *= cmd->param->move.move_fast_sense;
cmd->chassis.ctrl_vec.vy *= cmd->param->move.move_fast_sense;
}
if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_DECELEBRATE)) {
cmd->chassis.ctrl_vec.vx *= cmd->param->move.move_slow_sense;
cmd->chassis.ctrl_vec.vy *= cmd->param->move.move_slow_sense;
}
if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_FIRE)) {
/* 切换至开火模式，设置相应的射击频率和弹丸初速度 */
cmd->shoot.mode = SHOOT_MODE_LOADED;
cmd->shoot.fire = true;
} else {
/* 切换至准备模式，停止射击 */
cmd->shoot.mode = SHOOT_MODE_LOADED;
cmd->shoot.fire = false;
}
if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_FIRE_MODE)) {
/* 每按一次依次切换开火下一个模式 */
cmd->shoot.fire_mode++;
cmd->shoot.fire_mode %= FIRE_MODE_NUM;
}
if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_ROTOR)) {
/* 切换到小陀螺模式 */
cmd->chassis.mode = CHASSIS_MODE_ROTOR;
cmd->chassis.mode_rotor = ROTOR_MODE_RAND;
}
if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_OPENCOVER)) {
/* 每按一次开、关弹舱盖 */
cmd->shoot.cover_open = !cmd->shoot.cover_open;
}
if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_BUFF)) {
if (cmd->ai_status == AI_STATUS_HITSWITCH) {
/* 停止ai的打符模式，停用host控制 */
CMD_RefereeAdd(&(cmd->referee), CMD_UI_HIT_SWITCH_STOP);
cmd->host_overwrite = false;
cmd->ai_status = AI_STATUS_STOP;
} else if (cmd->ai_status == AI_STATUS_AUTOAIM) {
/* 自瞄模式中切换失败提醒 */
} else {
/* ai切换至打符模式，启用host控制 */
CMD_RefereeAdd(&(cmd->referee), CMD_UI_HIT_SWITCH_START);
cmd->ai_status = AI_STATUS_HITSWITCH;
cmd->host_overwrite = true;
}
}
if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_AUTOAIM)) {
if (cmd->ai_status == AI_STATUS_AUTOAIM) {
/* 停止ai的自瞄模式，停用host控制 */
cmd->host_overwrite = false;
cmd->ai_status = AI_STATUS_STOP;
CMD_RefereeAdd(&(cmd->referee), CMD_UI_AUTO_AIM_STOP);
} else {
/* ai切换至自瞄模式，启用host控制 */
cmd->ai_status = AI_STATUS_AUTOAIM;
cmd->host_overwrite = true;
CMD_RefereeAdd(&(cmd->referee), CMD_UI_AUTO_AIM_START);
}
} else {
cmd->host_overwrite = false;
// TODO: 修复逻辑
}
if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_REVTRIG)) {
/* 按下拨弹反转 */
cmd->shoot.reverse_trig = true;
}
if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_FOLLOWGIMBAL35)) {
cmd->chassis.mode = CHASSIS_MODE_FOLLOW_GIMBAL_35;
}
/* 保存当前按下的键位状态 */
cmd->key_last = rc->key;
memcpy(&(cmd->mouse_last), &(rc->mouse), sizeof(cmd->mouse_last));
}
/**
* @brief 解析rc行为逻辑
*
* @param rc 遥控器数据
* @param cmd 主结构体
* @param dt_sec 两次解析的间隔
*/
static void CMD_RcLogic(const CMD_RC_t *rc, CMD_t *cmd, float dt_sec) {
switch (rc->sw_l) {
/* 左拨杆相应行为选择和解析 */
case CMD_SW_UP:
cmd->chassis.mode = CHASSIS_MODE_BREAK;
break;
case CMD_SW_MID:
cmd->chassis.mode = CHASSIS_MODE_FOLLOW_GIMBAL;
break;
case CMD_SW_DOWN:
cmd->chassis.mode = CHASSIS_MODE_ROTOR;
cmd->chassis.mode_rotor = ROTOR_MODE_CW;
break;
case CMD_SW_ERR:
cmd->chassis.mode = CHASSIS_MODE_RELAX;
break;
}
switch (rc->sw_r) {
/* 右拨杆相应行为选择和解析*/
case CMD_SW_UP:
cmd->gimbal.mode = GIMBAL_MODE_ABSOLUTE;
cmd->shoot.mode = SHOOT_MODE_SAFE;
break;
case CMD_SW_MID:
cmd->gimbal.mode = GIMBAL_MODE_ABSOLUTE;
cmd->shoot.fire = false;
cmd->shoot.mode = SHOOT_MODE_LOADED;
break;
case CMD_SW_DOWN:
cmd->gimbal.mode = GIMBAL_MODE_ABSOLUTE;
cmd->shoot.mode = SHOOT_MODE_LOADED;
cmd->shoot.fire_mode = FIRE_MODE_SINGLE;
cmd->shoot.fire = true;
break;
/*
case CMD_SW_UP:
cmd->gimbal.mode = GIMBAL_MODE_RELAX;
cmd->shoot.mode = SHOOT_MODE_SAFE;
break;
case CMD_SW_MID:
cmd->gimbal.mode = GIMBAL_MODE_RELAX;
cmd->shoot.fire = false;
cmd->shoot.mode = SHOOT_MODE_LOADED;
break;
case CMD_SW_DOWN:
cmd->gimbal.mode = GIMBAL_MODE_RELAX;
cmd->shoot.mode = SHOOT_MODE_LOADED;
cmd->shoot.fire_mode = FIRE_MODE_SINGLE;
cmd->shoot.fire = true;
break;
*/
case CMD_SW_ERR:
cmd->gimbal.mode = GIMBAL_MODE_RELAX;
cmd->shoot.mode = SHOOT_MODE_RELAX;
}
/* 将操纵杆的对应值转换为底盘的控制向量和云台变化的欧拉角 */
cmd->chassis.ctrl_vec.vx = rc->ch_l_x;
cmd->chassis.ctrl_vec.vy = rc->ch_l_y;
cmd->gimbal.delta_eulr.yaw = rc->ch_r_x * dt_sec * cmd->param->sens_rc;
cmd->gimbal.delta_eulr.pit = rc->ch_r_y * dt_sec * cmd->param->sens_rc;
}
/**
* @brief rc失控时机器人恢复放松模式
*
* @param cmd 主结构体
*/
static void CMD_RcLostLogic(CMD_t *cmd) {
/* 机器人底盘、云台、射击运行模式恢复至放松模式 */
cmd->chassis.mode = CHASSIS_MODE_RELAX;
cmd->gimbal.mode = GIMBAL_MODE_RELAX;
cmd->shoot.mode = SHOOT_MODE_RELAX;
}
/**
* @brief 初始化命令解析
*
* @param cmd 主结构体
* @param param 参数
* @return int8_t 0对应没有错误
*/
int8_t CMD_Init(CMD_t *cmd, const CMD_Params_t *param) {
/* 指针检测 */
if (cmd == NULL) return -1;
if (param == NULL) return -1;
/* 设置机器人的命令参数，初始化控制方式为rc控制 */
cmd->pc_ctrl = false;
cmd->param = param;
return 0;
}
/**
* @brief 检查是否启用上位机控制指令覆盖
*
* @param cmd 主结构体
* @return true 启用
* @return false 不启用
*/
inline bool CMD_CheckHostOverwrite(CMD_t *cmd) { return cmd->host_overwrite; }
/**
* @brief 解析命令
*
* @param rc 遥控器数据
* @param cmd 命令
* @param dt_sec 两次解析的间隔
* @return int8_t 0对应没有错误
*/
int8_t CMD_ParseRc(CMD_RC_t *rc, CMD_t *cmd, float dt_sec) {
/* 指针检测 */
if (rc == NULL) return -1;
if (cmd == NULL) return -1;
/* 在pc控制和rc控制间切换 */
if (CMD_KeyPressedRc(rc, CMD_KEY_SHIFT) &&
CMD_KeyPressedRc(rc, CMD_KEY_CTRL) && CMD_KeyPressedRc(rc, CMD_KEY_Q))
cmd->pc_ctrl = true;
if (CMD_KeyPressedRc(rc, CMD_KEY_SHIFT) &&
CMD_KeyPressedRc(rc, CMD_KEY_CTRL) && CMD_KeyPressedRc(rc, CMD_KEY_E))
cmd->pc_ctrl = false;
/*c当rc丢控时，恢复机器人至默认状态 */
if ((rc->sw_l == CMD_SW_ERR) || (rc->sw_r == CMD_SW_ERR)) {
CMD_RcLostLogic(cmd);
} else {
if (cmd->pc_ctrl) {
CMD_PcLogic(rc, cmd, dt_sec);
} else {
CMD_RcLogic(rc, cmd, dt_sec);
}
}
return 0;
}
/**
* @brief 解析上位机命令
*
* @param host host数据
* @param cmd 命令
* @param dt_sec 两次解析的间隔
* @return int8_t 0对应没有错误
*/
int8_t CMD_ParseHost(const CMD_Host_t *host, CMD_t *cmd, float dt_sec) {
(void)dt_sec; /* 未使用dt_sec，消除警告 */
/* 指针检测 */
if (host == NULL) return -1;
if (cmd == NULL) return -1;
/* 云台欧拉角设置为host相应的变化的欧拉角 */
cmd->gimbal.delta_eulr.yaw = host->gimbal_delta.yaw;
cmd->gimbal.delta_eulr.pit = host->gimbal_delta.pit;
/* host射击命令，设置不同的射击频率和弹丸初速度 */
if (host->fire) {
cmd->shoot.mode = SHOOT_MODE_LOADED;
cmd->shoot.fire = true;
} else {
cmd->shoot.mode = SHOOT_MODE_SAFE;
}
return 0;
}
/**
* @brief 添加向Referee发送的命令
*
* @param ref 命令队列
* @param cmd 要添加的命令
* @return int8_t 0对应没有错误
*/
int8_t CMD_RefereeAdd(CMD_RefereeCmd_t *ref, CMD_UI_t cmd) {
/* 指针检测 */
if (ref == NULL) return -1;
/* 越界检测 */
if (ref->counter >= CMD_REFEREE_MAX_NUM || ref->counter < 0) return -1;
/* 添加机器人当前行为状态到画图的命令队列中 */
ref->cmd[ref->counter] = cmd;
ref->counter++;
return 0;
}

306
User/component/cmd.h
View File

@ -1,306 +0,0 @@
/*
控制命令
*/
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
#include <stdbool.h>
#include <stdint.h>
#include "ahrs.h"
#define CMD_REFEREE_MAX_NUM (3) /* 发送命令限定的最大数量 */
/* 机器人型号 */
typedef enum {
ROBOT_MODEL_INFANTRY = 0, /* 步兵机器人 */
ROBOT_MODEL_HERO, /* 英雄机器人 */
ROBOT_MODEL_ENGINEER, /* 工程机器人 */
ROBOT_MODEL_DRONE, /* 空中机器人 */
ROBOT_MODEL_SENTRY, /* 哨兵机器人 */
ROBOT_MODEL_NUM, /* 型号数量 */
} CMD_RobotModel_t;
/* 底盘运行模式 */
typedef enum {
CHASSIS_MODE_RELAX, /* 放松模式，电机不输出。一般情况底盘初始化之后的模式 */
CHASSIS_MODE_BREAK, /* 刹车模式，电机闭环控制保持静止。用于机器人停止状态 */
CHASSIS_MODE_FOLLOW_GIMBAL, /* 通过闭环控制使车头方向跟随云台 */
CHASSIS_MODE_FOLLOW_GIMBAL_35, /* 通过闭环控制使车头方向35度跟随云台 */
CHASSIS_MODE_ROTOR, /* 小陀螺模式，通过闭环控制使底盘不停旋转 */
CHASSIS_MODE_INDENPENDENT, /* 独立模式。底盘运行不受云台影响 */
CHASSIS_MODE_OPEN, /* 开环模式。底盘运行不受PID控制，直接输出到电机 */
} CMD_ChassisMode_t;
/* 云台运行模式 */
typedef enum {
GIMBAL_MODE_RELAX, /* 放松模式，电机不输出。一般情况云台初始化之后的模式 */
GIMBAL_MODE_ABSOLUTE, /* 绝对坐标系控制，控制在空间内的绝对姿态 */
GIMBAL_MODE_RELATIVE, /* 相对坐标系控制，控制相对于底盘的姿态 */
} CMD_GimbalMode_t;
/* 射击运行模式 */
typedef enum {
SHOOT_MODE_RELAX, /* 放松模式，电机不输出 */
SHOOT_MODE_SAFE, /* 保险模式，电机闭环控制保持静止 */
SHOOT_MODE_LOADED, /* 上膛模式，摩擦轮开启。随时准备开火 */
} CMD_ShootMode_t;
typedef enum {
FIRE_MODE_SINGLE, /* 单发开火模式 */
FIRE_MODE_BURST, /* N连发开火模式 */
FIRE_MODE_CONT, /* 持续开火模式 */
FIRE_MODE_NUM,
} CMD_FireMode_t;
/* 小陀螺转动模式 */
typedef enum {
ROTOR_MODE_CW, /* 顺时针转动 */
ROTOR_MODE_CCW, /* 逆时针转动 */
ROTOR_MODE_RAND, /* 随机转动 */
} CMD_RotorMode_t;
/* 底盘控制命令 */
typedef struct {
CMD_ChassisMode_t mode; /* 底盘运行模式 */
CMD_RotorMode_t mode_rotor; /* 小陀螺转动模式 */
MoveVector_t ctrl_vec; /* 底盘控制向量 */
} CMD_ChassisCmd_t;
/* 云台控制命令 */
typedef struct {
CMD_GimbalMode_t mode; /* 云台运行模式 */
AHRS_Eulr_t delta_eulr; /* 欧拉角变化角度 */
} CMD_GimbalCmd_t;
/* 射击控制命令 */
typedef struct {
CMD_ShootMode_t mode; /* 射击运行模式 */
CMD_FireMode_t fire_mode; /* 开火模式 */
bool fire; /*开火*/
bool cover_open; /* 弹舱盖开关 */
bool reverse_trig; /* 拨弹电机状态 */
} CMD_ShootCmd_t;
/* 拨杆位置 */
typedef enum {
CMD_SW_ERR = 0,
CMD_SW_UP = 1,
CMD_SW_MID = 3,
CMD_SW_DOWN = 2,
} CMD_SwitchPos_t;
/* 键盘按键值 */
typedef enum {
CMD_KEY_W = 0,
CMD_KEY_S,
CMD_KEY_A,
CMD_KEY_D,
CMD_KEY_SHIFT,
CMD_KEY_CTRL,
CMD_KEY_Q,
CMD_KEY_E,
CMD_KEY_R,
CMD_KEY_F,
CMD_KEY_G,
CMD_KEY_Z,
CMD_KEY_X,
CMD_KEY_C,
CMD_KEY_V,
CMD_KEY_B,
CMD_L_CLICK,
CMD_R_CLICK,
CMD_KEY_NUM,
} CMD_KeyValue_t;
/* 行为值序列 */
typedef enum {
CMD_BEHAVIOR_FORE = 0, /* 向前 */
CMD_BEHAVIOR_BACK, /* 向后 */
CMD_BEHAVIOR_LEFT, /* 向左 */
CMD_BEHAVIOR_RIGHT, /* 向右 */
CMD_BEHAVIOR_ACCELERATE, /* 加速 */
CMD_BEHAVIOR_DECELEBRATE, /* 减速 */
CMD_BEHAVIOR_FIRE, /* 开火 */
CMD_BEHAVIOR_FIRE_MODE, /* 切换开火模式 */
CMD_BEHAVIOR_BUFF, /* 打符模式 */
CMD_BEHAVIOR_AUTOAIM, /* 自瞄模式 */
CMD_BEHAVIOR_OPENCOVER, /* 弹舱盖开关 */
CMD_BEHAVIOR_ROTOR, /* 小陀螺模式 */
CMD_BEHAVIOR_REVTRIG, /* 反转拨弹 */
CMD_BEHAVIOR_FOLLOWGIMBAL35, /* 跟随云台呈35度 */
CMD_BEHAVIOR_NUM,
} CMD_Behavior_t;
typedef enum {
CMD_ACTIVE_PRESSING, /* 按下时触发 */
CMD_ACTIVE_RASING, /* 抬起时触发 */
CMD_ACTIVE_PRESSED, /* 按住时触发 */
} CMD_ActiveType_t;
typedef struct {
CMD_ActiveType_t active;
CMD_KeyValue_t key;
} CMD_KeyMapItem_t;
/* 行为映射的对应按键数组 */
typedef struct {
CMD_KeyMapItem_t key_map[CMD_BEHAVIOR_NUM];
} CMD_KeyMap_Params_t;
/* 位移灵敏度参数 */
typedef struct {
float move_sense; /* 移动灵敏度 */
float move_fast_sense; /* 加速灵敏度 */
float move_slow_sense; /* 减速灵敏度 */
} CMD_Move_Params_t;
typedef struct {
uint16_t width;
uint16_t height;
} CMD_Screen_t;
/* 命令参数 */
typedef struct {
float sens_mouse; /* 鼠标灵敏度 */
float sens_rc; /* 遥控器摇杆灵敏度 */
CMD_KeyMap_Params_t map; /* 按键映射行为命令 */
CMD_Move_Params_t move; /* 位移灵敏度参数 */
CMD_Screen_t screen; /* 屏幕分辨率参数 */
} CMD_Params_t;
/* AI行为状态 */
typedef enum {
AI_STATUS_STOP, /* 停止状态 */
AI_STATUS_AUTOAIM, /* 自瞄状态 */
AI_STATUS_HITSWITCH, /* 打符状态 */
AI_STATUS_AUTOMATIC /* 自动状态 */
} CMD_AI_Status_t;
/* UI所用行为状态 */
typedef enum {
CMD_UI_NOTHING, /* 当前无状态 */
CMD_UI_AUTO_AIM_START, /* 自瞄状态开启 */
CMD_UI_AUTO_AIM_STOP, /* 自瞄状态关闭 */
CMD_UI_HIT_SWITCH_START, /* 打符状态开启 */
CMD_UI_HIT_SWITCH_STOP /* 打符状态关闭 */
} CMD_UI_t;
/*裁判系统发送的命令*/
typedef struct {
CMD_UI_t cmd[CMD_REFEREE_MAX_NUM]; /* 命令数组 */
uint8_t counter; /* 命令计数 */
} CMD_RefereeCmd_t;
typedef struct {
bool pc_ctrl; /* 是否使用键鼠控制 */
bool host_overwrite; /* 是否Host控制 */
uint16_t key_last; /* 上次按键键值 */
struct {
int16_t x;
int16_t y;
int16_t z;
bool l_click; /* 左键 */
bool r_click; /* 右键 */
} mouse_last; /* 鼠标值 */
CMD_AI_Status_t ai_status; /* AI状态 */
const CMD_Params_t *param; /* 命令参数 */
CMD_ChassisCmd_t chassis; /* 底盘控制命令 */
CMD_GimbalCmd_t gimbal; /* 云台控制命令 */
CMD_ShootCmd_t shoot; /* 射击控制命令 */
CMD_RefereeCmd_t referee; /* 裁判系统发送命令 */
} CMD_t;
typedef struct {
float ch_l_x; /* 遥控器左侧摇杆横轴值，上为正 */
float ch_l_y; /* 遥控器左侧摇杆纵轴值，右为正 */
float ch_r_x; /* 遥控器右侧摇杆横轴值，上为正 */
float ch_r_y; /* 遥控器右侧摇杆纵轴值，右为正 */
float ch_res; /* 第五通道值 */
CMD_SwitchPos_t sw_r; /* 右侧拨杆位置 */
CMD_SwitchPos_t sw_l; /* 左侧拨杆位置 */
struct {
int16_t x;
int16_t y;
int16_t z;
bool l_click; /* 左键 */
bool r_click; /* 右键 */
} mouse; /* 鼠标值 */
uint16_t key; /* 按键值 */
uint16_t res; /* 保留，未启用 */
} CMD_RC_t;
typedef struct {
AHRS_Eulr_t gimbal_delta; /* 欧拉角的变化量 */
struct {
float vx; /* x轴移动速度 */
float vy; /* y轴移动速度 */
float wz; /* z轴转动速度 */
} chassis_move_vec; /* 底盘移动向量 */
bool fire; /* 开火状态 */
} CMD_Host_t;
/**
* @brief 解析行为命令
*
* @param rc 遥控器数据
* @param cmd 主结构体
*/
int8_t CMD_Init(CMD_t *cmd, const CMD_Params_t *param);
/**
* @brief 检查是否启用上位机控制指令覆盖
*
* @param cmd 主结构体
* @return true 启用
* @return false 不启用
*/
bool CMD_CheckHostOverwrite(CMD_t *cmd);
/**
* @brief 解析命令
*
* @param rc 遥控器数据
* @param cmd 命令
* @param dt_sec 两次解析的间隔
* @return int8_t 0对应没有错误
*/
int8_t CMD_ParseRc(CMD_RC_t *rc, CMD_t *cmd, float dt_sec);
/**
* @brief 解析上位机命令
*
* @param host host数据
* @param cmd 命令
* @param dt_sec 两次解析的间隔
* @return int8_t 0对应没有错误
*/
int8_t CMD_ParseHost(const CMD_Host_t *host, CMD_t *cmd, float dt_sec);
/**
* @brief 添加向Referee发送的命令
*
* @param ref 命令队列
* @param cmd 要添加的命令
* @return int8_t 0对应没有错误
*/
int8_t CMD_RefereeAdd(CMD_RefereeCmd_t *ref, CMD_UI_t cmd);
#ifdef __cplusplus
}
#endif

21
User/component/component_config.yaml
View File

@ -1,21 +0,0 @@
ahrs:
dependencies:
- component/filter
enabled: true
cmd:
dependencies: []
enabled: true
filter:
dependencies:
- component/ahrs
enabled: true
limiter:
dependencies: []
enabled: true
pid:
dependencies:
- component/filter
enabled: true
user_math:
dependencies: []
enabled: true

185
User/component/filter.c
View File

@ -1,185 +0,0 @@
/*
各类滤波器。
*/
#include "filter.h"
#include "user_math.h"
/**
* @brief 初始化滤波器
*
* @param f 滤波器
* @param sample_freq 采样频率
* @param cutoff_freq 截止频率
*/
void LowPassFilter2p_Init(LowPassFilter2p_t *f, float sample_freq,
float cutoff_freq) {
if (f == NULL) return;
f->cutoff_freq = cutoff_freq;
f->delay_element_1 = 0.0f;
f->delay_element_2 = 0.0f;
if (f->cutoff_freq <= 0.0f) {
/* no filtering */
f->b0 = 1.0f;
f->b1 = 0.0f;
f->b2 = 0.0f;
f->a1 = 0.0f;
f->a2 = 0.0f;
return;
}
const float fr = sample_freq / f->cutoff_freq;
const float ohm = tanf(M_PI / fr);
const float c = 1.0f + 2.0f * cosf(M_PI / 4.0f) * ohm + ohm * ohm;
f->b0 = ohm * ohm / c;
f->b1 = 2.0f * f->b0;
f->b2 = f->b0;
f->a1 = 2.0f * (ohm * ohm - 1.0f) / c;
f->a2 = (1.0f - 2.0f * cosf(M_PI / 4.0f) * ohm + ohm * ohm) / c;
}
/**
* @brief 施加一次滤波计算
*
* @param f 滤波器
* @param sample 采样的值
* @return float 滤波后的值
*/
float LowPassFilter2p_Apply(LowPassFilter2p_t *f, float sample) {
if (f == NULL) return 0.0f;
/* do the filtering */
float delay_element_0 =
sample - f->delay_element_1 * f->a1 - f->delay_element_2 * f->a2;
if (isinf(delay_element_0)) {
/* don't allow bad values to propagate via the filter */
delay_element_0 = sample;
}
const float output = delay_element_0 * f->b0 + f->delay_element_1 * f->b1 +
f->delay_element_2 * f->b2;
f->delay_element_2 = f->delay_element_1;
f->delay_element_1 = delay_element_0;
/* return the value. Should be no need to check limits */
return output;
}
/**
* @brief 重置滤波器
*
* @param f 滤波器
* @param sample 采样的值
* @return float 滤波后的值
*/
float LowPassFilter2p_Reset(LowPassFilter2p_t *f, float sample) {
if (f == NULL) return 0.0f;
const float dval = sample / (f->b0 + f->b1 + f->b2);
if (isfinite(dval)) {
f->delay_element_1 = dval;
f->delay_element_2 = dval;
} else {
f->delay_element_1 = sample;
f->delay_element_2 = sample;
}
return LowPassFilter2p_Apply(f, sample);
}
/**
* @brief 初始化滤波器
*
* @param f 滤波器
* @param sample_freq 采样频率
* @param notch_freq 中心频率
* @param bandwidth 带宽
*/
void NotchFilter_Init(NotchFilter_t *f, float sample_freq, float notch_freq,
float bandwidth) {
if (f == NULL) return;
f->notch_freq = notch_freq;
f->bandwidth = bandwidth;
f->delay_element_1 = 0.0f;
f->delay_element_2 = 0.0f;
if (notch_freq <= 0.0f) {
/* no filtering */
f->b0 = 1.0f;
f->b1 = 0.0f;
f->b2 = 0.0f;
f->a1 = 0.0f;
f->a2 = 0.0f;
return;
}
const float alpha = tanf(M_PI * bandwidth / sample_freq);
const float beta = -cosf(M_2PI * notch_freq / sample_freq);
const float a0_inv = 1.0f / (alpha + 1.0f);
f->b0 = a0_inv;
f->b1 = 2.0f * beta * a0_inv;
f->b2 = a0_inv;
f->a1 = f->b1;
f->a2 = (1.0f - alpha) * a0_inv;
}
/**
* @brief 施加一次滤波计算
*
* @param f 滤波器
* @param sample 采样的值
* @return float 滤波后的值
*/
inline float NotchFilter_Apply(NotchFilter_t *f, float sample) {
if (f == NULL) return 0.0f;
/* Direct Form II implementation */
const float delay_element_0 =
sample - f->delay_element_1 * f->a1 - f->delay_element_2 * f->a2;
const float output = delay_element_0 * f->b0 + f->delay_element_1 * f->b1 +
f->delay_element_2 * f->b2;
f->delay_element_2 = f->delay_element_1;
f->delay_element_1 = delay_element_0;
return output;
}
/**
* @brief 重置滤波器
*
* @param f 滤波器
* @param sample 采样的值
* @return float 滤波后的值
*/
float NotchFilter_Reset(NotchFilter_t *f, float sample) {
if (f == NULL) return 0.0f;
float dval = sample;
if (fabsf(f->b0 + f->b1 + f->b2) > FLT_EPSILON) {
dval = dval / (f->b0 + f->b1 + f->b2);
}
f->delay_element_1 = dval;
f->delay_element_2 = dval;
return NotchFilter_Apply(f, sample);
}

104
User/component/filter.h
View File

@ -1,104 +0,0 @@
/*
各类滤波器。
*/
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
#include "user_math.h"
/* 二阶低通滤波器 */
typedef struct {
float cutoff_freq; /* 截止频率 */
float a1;
float a2;
float b0;
float b1;
float b2;
float delay_element_1;
float delay_element_2;
} LowPassFilter2p_t;
/* 带阻滤波器 */
typedef struct {
float notch_freq; /* 阻止频率 */
float bandwidth; /* 带宽 */
float a1;
float a2;
float b0;
float b1;
float b2;
float delay_element_1;
float delay_element_2;
} NotchFilter_t;
/**
* @brief 初始化滤波器
*
* @param f 滤波器
* @param sample_freq 采样频率
* @param cutoff_freq 截止频率
*/
void LowPassFilter2p_Init(LowPassFilter2p_t *f, float sample_freq,
float cutoff_freq);
/**
* @brief 施加一次滤波计算
*
* @param f 滤波器
* @param sample 采样的值
* @return float 滤波后的值
*/
float LowPassFilter2p_Apply(LowPassFilter2p_t *f, float sample);
/**
* @brief 重置滤波器
*
* @param f 滤波器
* @param sample 采样的值
* @return float 滤波后的值
*/
float LowPassFilter2p_Reset(LowPassFilter2p_t *f, float sample);
/**
* @brief 初始化滤波器
*
* @param f 滤波器
* @param sample_freq 采样频率
* @param notch_freq 中心频率
* @param bandwidth 带宽
*/
void NotchFilter_Init(NotchFilter_t *f, float sample_freq, float notch_freq,
float bandwidth);
/**
* @brief 施加一次滤波计算
*
* @param f 滤波器
* @param sample 采样的值
* @return float 滤波后的值
*/
float NotchFilter_Apply(NotchFilter_t *f, float sample);
/**
* @brief 重置滤波器
*
* @param f 滤波器
* @param sample 采样的值
* @return float 滤波后的值
*/
float NotchFilter_Reset(NotchFilter_t *f, float sample);
#ifdef __cplusplus
}
#endif

113
User/component/kinematics.c
View File

@ -1,113 +0,0 @@
/*
一些运动学相关的计算函数
*/
#include "component/kinematics.h"
#include "component/user_math.h"
/*串联腿单腿转摆动杆正运动学*/
/*大腿小腿均按照水平为0点，向下为正方向*/
/**
* @brief 串联腿单腿转摆动杆正运动学
* @param param 腿部长度参数
* @param hip_angle 髋关节角度(弧度)
* @param knee_angle 膝关节角度(弧度)
* @param angle 计算到的等效摆动杆角度
* @param heigh 计算到的等效摆动杆高度
* @return
*/
int8_t KIN_SerialLeg_FK(const KIN_SerialLeg_Param_t *param, const float *hip_angle,const float *knee_angle, float *angle, float *height) {
if (param == NULL || hip_angle == NULL || knee_angle == NULL || angle == NULL || height == NULL) {
return -1; // 参数错误
}
float L1 = param->thigh_length;
float L2 = param->calf_length;
float q1 = *hip_angle; // 髋关节角度
float q2 = *knee_angle; // 膝关节角度
float q4 = (M_PI - q1 - q2)/2;
// 使用余弦定理求解等效摆动杆长度
// L2*L2 = L1*L1 + L3*L3 - 2*L1*L3*cosf(q4);
// 整理得：L3*L3 - 2*L1*cosf(q4)*L3 + (L1*L1 - L2*L2) = 0
float a = 1.0f;
float b = -2.0f * L1 * cosf(q4);
float c = L1*L1 - L2*L2;
float discriminant = b*b - 4*a*c;
// 检查判别式，确保有实数解
if (discriminant < 0) {
return -2; // 无实数解，配置不可达
}
float sqrt_discriminant = sqrtf(discriminant);
float L3_1 = (-b + sqrt_discriminant) / (2*a);
float L3_2 = (-b - sqrt_discriminant) / (2*a);
// 选择正的解（物理意义上的长度）
float L3;
if (L3_1 > 0 && L3_2 > 0) {
// 两个正解，选择较小的（通常对应机构的正常工作姿态）
L3 = (L3_1 < L3_2) ? L3_1 : L3_2;
} else if (L3_1 > 0) {
L3 = L3_1;
} else if (L3_2 > 0) {
L3 = L3_2;
} else {
return -3; // 无正解
}
*angle = q1 + q4;
*height = L3;
return 0; // 成功
}
/*串联腿单腿转摆动杆逆运动学*/
/*大腿小腿均按照水平为0点，向下为正方向*/
/**
* @brief 串联腿单腿转摆动杆逆运动学
* @param param 腿部长度参数
* @param angle 输入的等效摆动杆角度
* @param height 输入的等效摆动杆高度
* @param hip_angle 计算到的髋关节角度(弧度)
* @param knee_angle 计算到的膝关节角度(弧度)
* @return
*/
int8_t KIN_SerialLeg_IK(const KIN_SerialLeg_Param_t *param, const float *angle,const float *height, float *hip_angle, float *knee_angle) {
if (param == NULL || angle == NULL || height == NULL || hip_angle == NULL || knee_angle == NULL) {
return -1; // 参数错误
}
float L1 = param->thigh_length;
float L2 = param->calf_length;
float q = *angle; // 摆动杆角度
float h = *height; // 摆动杆长度
// 由正解可知：q = q1 + q4，h = L3
// 由余弦定理：L2^2 = L1^2 + h^2 - 2*L1*h*cos(q4)
// 整理得：cos(q4) = (L1^2 + h^2 - L2^2) / (2*L1*h)
float cos_q4 = (L1*L1 + h*h - L2*L2) / (2.0f * L1 * h);
// 检查 cos_q4 是否在 [-1, 1] 范围内
if (cos_q4 < -1.0f || cos_q4 > 1.0f) {
return -2; // 不可达
}
float q4 = acosf(cos_q4);
// 由正解：q = q1 + q4，且 q4 = (PI - q1 - q2)/2
// 整理得：q1 = q - q4
float q1 = q - q4;
// 再由 q4 = (PI - q1 - q2)/2，整理得：q2 = PI - 2*q4 - q1
float q2 = M_PI - 2.0f * q4 - q1;
*hip_angle = q1;
*knee_angle = q2;
return 0; // 成功
}

43
User/component/kinematics.h
View File

@ -1,43 +0,0 @@
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ----------------------------------------------------------------- */
#include <stdint.h>
#include <stdbool.h>
#include <math.h>
/**
* @brief 串联腿参数结构体
*/
typedef struct {
float thigh_length; // 大腿长度
float calf_length; // 小腿长度
} KIN_SerialLeg_Param_t; // ← 补上分号
/**
* @brief 串联腿单腿转摆动杆正运动学
* @param param 腿部长度参数
* @param hip_angle 髋关节角度(弧度)
* @param knee_angle 膝关节角度(弧度)
* @param angle 计算到的等效摆动杆角度
* @param height 计算到的等效摆动杆高度
* @return 0:成功, -1:失败
*/
int8_t KIN_SerialLeg_FK(const KIN_SerialLeg_Param_t *param, const float *hip_angle, const float *knee_angle, float *angle, float *height);
/**
* @brief 串联腿单腿转摆动杆逆运动学
* @param param 腿部长度参数
* @param angle 输入的等效摆动杆角度
* @param height 输入的等效摆动杆高度
* @param hip_angle 计算到的髋关节角度(弧度)
* @param knee_angle 计算到的膝关节角度(弧度)
* @return 0:成功, -1:失败
*/
int8_t KIN_SerialLeg_IK(const KIN_SerialLeg_Param_t *param, const float *angle, const float *height, float *hip_angle, float *knee_angle);
#ifdef __cplusplus
}
#endif

107
User/component/limiter.c
View File

@ -1,107 +0,0 @@
/*
限制器
*/
#include "limiter.h"
#include <math.h>
#include <stddef.h>
#define POWER_BUFF_THRESHOLD 20
#define CHASSIS_POWER_CHECK_FREQ 10
#define CHASSIS_POWER_FACTOR_PASS 0.9f
#define CHASSIS_POWER_FACTOR_NO_PASS 1.5f
#define CHASSIS_MOTOR_CIRCUMFERENCE 0.12f
/**
* @brief 限制底盘功率不超过power_limit
*
* @param power_limit 最大功率
* @param motor_out 电机输出值
* @param speed 电机转速
* @param len 电机数量
* @return int8_t 0对应没有错误
*/
int8_t PowerLimit_ChassicOutput(float power_limit, float *motor_out,
float *speed, uint32_t len) {
/* power_limit小于0时不进行限制 */
if (motor_out == NULL || speed == NULL || power_limit < 0) return -1;
float sum_motor_out = 0.0f;
for (uint32_t i = 0; i < len; i++) {
/* 总功率计算 P=F(由转矩电流表示)*V(由转速表示) */
sum_motor_out +=
fabsf(motor_out[i]) * fabsf(speed[i]) * CHASSIS_MOTOR_CIRCUMFERENCE;
}
/* 保持每个电机输出值缩小时比例不变 */
if (sum_motor_out > power_limit) {
for (uint32_t i = 0; i < len; i++) {
motor_out[i] *= power_limit / sum_motor_out;
}
}
return 0;
}
/**
* @brief 电容输入功率计算
*
* @param power_in 底盘当前功率
* @param power_limit 裁判系统功率限制值
* @param power_buffer 缓冲能量
* @return float 裁判系统输出最大值
*/
float PowerLimit_CapInput(float power_in, float power_limit,
float power_buffer) {
float target_power = 0.0f;
/* 计算下一个检测周期的剩余缓冲能量 */
float heat_buff = power_buffer - (float)(power_in - power_limit) /
(float)CHASSIS_POWER_CHECK_FREQ;
if (heat_buff < POWER_BUFF_THRESHOLD) { /* 功率限制 */
target_power = power_limit * CHASSIS_POWER_FACTOR_PASS;
} else {
target_power = power_limit * CHASSIS_POWER_FACTOR_NO_PASS;
}
return target_power;
}
/**
* @brief 使用缓冲能量计算底盘最大功率
*
* @param power_limit 裁判系统功率限制值
* @param power_buffer 缓冲能量
* @return float 底盘输出最大值
*/
float PowerLimit_TargetPower(float power_limit, float power_buffer) {
float target_power = 0.0f;
/* 根据剩余缓冲能量计算输出功率 */
target_power = power_limit * (power_buffer - 10.0f) / 20.0f;
if (target_power < 0.0f) target_power = 0.0f;
return target_power;
}
/**
* @brief 射击频率控制
*
* @param heat 当前热量
* @param heat_limit 热量上限
* @param cooling_rate 冷却速率
* @param heat_increase 冷却增加
* @param shoot_freq 经过热量限制后的射击频率
* @return float 射击频率
*/
float HeatLimit_ShootFreq(float heat, float heat_limit, float cooling_rate,
float heat_increase, bool is_big) {
float heat_percent = heat / heat_limit;
float stable_freq = cooling_rate / heat_increase;
if (is_big)
return stable_freq;
else
return (heat_percent > 0.7f) ? stable_freq : 3.0f * stable_freq;
}

55
User/component/limiter.h
View File

@ -1,55 +0,0 @@
/*
限制器
*/
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
#include <stdbool.h>
#include <stdint.h>
/**
* @brief 限制底盘功率不超过power_limit
*
* @param power_limit 最大功率
* @param motor_out 电机输出值
* @param speed 电机转速
* @param len 电机数量
* @return int8_t 0对应没有错误
*/
int8_t PowerLimit_ChassicOutput(float power_limit, float *motor_out,
float *speed, uint32_t len);
/**
* @brief 电容输入功率计算
*
* @param power_in 底盘当前功率
* @param power_limit 裁判系统功率限制值
* @param power_buffer 缓冲能量
* @return float 裁判系统输出最大值
*/
float PowerLimit_CapInput(float power_in, float power_limit,
float power_buffer);
/**
* @brief 使用缓冲能量计算底盘最大功率
*
* @param power_limit 裁判系统功率限制值
* @param power_buffer 缓冲能量
* @return float 底盘输出最大值
*/
float PowerLimit_TargetPower(float power_limit, float power_buffer);
/**
* @brief 射击频率控制
*
* @param heat 当前热量
* @param heat_limit 热量上限
* @param cooling_rate 冷却速率
* @param heat_increase 冷却增加
* @param shoot_freq 经过热量限制后的射击频率
* @return float 射击频率
*/
float HeatLimit_ShootFreq(float heat, float heat_limit, float cooling_rate,
float heat_increase, bool is_big);

229
User/component/lqr.c
View File

@ -1,229 +0,0 @@
/*
* LQR线性二次型最优控制器简化实现
*
* 本文件实现了轮腿机器人的LQR (Linear Quadratic Regulator) 控制算法
* 主要功能包括:
* 1. 状态反馈控制
* 2. 增益矩阵K计算控制输出
* 3. 控制输出限幅
*
* 系统模型:
* u = -K*(x - x_ref) (状态反馈)
*/
#include "lqr.h"
#include <string.h>
/* Private typedef ---------------------------------------------------------- */
/* Private define ----------------------------------------------------------- */
#define LQR_EPSILON (1e-6f) // 数值计算精度
#define LQR_DEFAULT_MAX_WHEEL (50.0f) // 默认最大轮毂力矩 (N*m)
#define LQR_DEFAULT_MAX_JOINT (30.0f) // 默认最大关节力矩 (N*m)
/* Private macro ------------------------------------------------------------ */
/* Private variables -------------------------------------------------------- */
/* Private function prototypes ---------------------------------------------- */
static void LQR_MatrixMultiply(const float K[4][10], const float state_error[10], float result[4]);
static float LQR_ComputeStateError(float current, float reference);
/* Exported functions ------------------------------------------------------- */
/**
* @brief 初始化LQR控制器
*/
int8_t LQR_Init(LQR_Controller_t *lqr, float max_wheel_torque, float max_joint_torque) {
if (lqr == NULL || max_wheel_torque <= 0 || max_joint_torque <= 0) {
return -1;
}
// 设置力矩限制
lqr->max_wheel_torque = max_wheel_torque;
lqr->max_joint_torque = max_joint_torque;
// 重置状态
LQR_Reset(lqr);
lqr->initialized = true;
return 0;
}
/**
* @brief 设置固定LQR增益矩阵
*/
int8_t LQR_SetGainMatrix(LQR_Controller_t *lqr, const LQR_GainMatrix_t *K) {
if (lqr == NULL || !lqr->initialized || K == NULL) {
return -1;
}
// 复制增益矩阵
memcpy(&lqr->K, K, sizeof(LQR_GainMatrix_t));
return 0;
}
/**
* @brief 更新机器人状态
*/
int8_t LQR_UpdateState(LQR_Controller_t *lqr, const LQR_State_t *state) {
if (lqr == NULL || !lqr->initialized || state == NULL) {
return -1;
}
// 复制状态，并对角度进行归一化
lqr->state = *state;
LQR_ANGLE_NORMALIZE(lqr->state.phi);
LQR_ANGLE_NORMALIZE(lqr->state.theta_ll);
LQR_ANGLE_NORMALIZE(lqr->state.theta_lr);
LQR_ANGLE_NORMALIZE(lqr->state.theta_b);
return 0;
}
/**
* @brief 设置参考状态
*/
int8_t LQR_SetReference(LQR_Controller_t *lqr, const LQR_State_t *reference) {
if (lqr == NULL || !lqr->initialized || reference == NULL) {
return -1;
}
// 复制参考状态，并对角度进行归一化
lqr->reference = *reference;
LQR_ANGLE_NORMALIZE(lqr->reference.phi);
LQR_ANGLE_NORMALIZE(lqr->reference.theta_ll);
LQR_ANGLE_NORMALIZE(lqr->reference.theta_lr);
LQR_ANGLE_NORMALIZE(lqr->reference.theta_b);
return 0;
}
/**
* @brief 计算LQR控制输出
*
* 实现状态反馈控制律: u = -K*(x - x_ref)
*/
int8_t LQR_ComputeControl(LQR_Controller_t *lqr) {
if (lqr == NULL || !lqr->initialized) {
return -1;
}
// 计算状态误差向量
float state_error[10];
state_error[0] = LQR_ComputeStateError(lqr->state.s, lqr->reference.s);
state_error[1] = LQR_ComputeStateError(lqr->state.ds, lqr->reference.ds);
state_error[2] = LQR_ComputeStateError(lqr->state.phi, lqr->reference.phi);
state_error[3] = LQR_ComputeStateError(lqr->state.dphi, lqr->reference.dphi);
state_error[4] = LQR_ComputeStateError(lqr->state.theta_ll, lqr->reference.theta_ll);
state_error[5] = LQR_ComputeStateError(lqr->state.dtheta_ll, lqr->reference.dtheta_ll);
state_error[6] = LQR_ComputeStateError(lqr->state.theta_lr, lqr->reference.theta_lr);
state_error[7] = LQR_ComputeStateError(lqr->state.dtheta_lr, lqr->reference.dtheta_lr);
state_error[8] = LQR_ComputeStateError(lqr->state.theta_b, lqr->reference.theta_b);
state_error[9] = LQR_ComputeStateError(lqr->state.dtheta_b, lqr->reference.dtheta_b);
// 计算控制输出: u = -K * (x - x_ref)
float control_vector[4];
LQR_MatrixMultiply(lqr->K.K, state_error, control_vector);
// 应用负反馈并限幅
lqr->control.T_wl = LQR_CLAMP(-control_vector[0], -lqr->max_wheel_torque, lqr->max_wheel_torque);
lqr->control.T_wr = LQR_CLAMP(-control_vector[1], -lqr->max_wheel_torque, lqr->max_wheel_torque);
lqr->control.T_bl = LQR_CLAMP(-control_vector[2], -lqr->max_joint_torque, lqr->max_joint_torque);
lqr->control.T_br = LQR_CLAMP(-control_vector[3], -lqr->max_joint_torque, lqr->max_joint_torque);
return 0;
}
/**
* @brief 获取控制输出
*/
int8_t LQR_GetControl(const LQR_Controller_t *lqr, LQR_Control_t *control) {
if (lqr == NULL || !lqr->initialized || control == NULL) {
return -1;
}
*control = lqr->control;
return 0;
}
/**
* @brief 重置LQR控制器状态
*/
void LQR_Reset(LQR_Controller_t *lqr) {
if (lqr == NULL) {
return;
}
// 清零状态和控制量
memset(&lqr->state, 0, sizeof(LQR_State_t));
memset(&lqr->reference, 0, sizeof(LQR_State_t));
memset(&lqr->control, 0, sizeof(LQR_Control_t));
memset(&lqr->K, 0, sizeof(LQR_GainMatrix_t));
}
/**
* @brief 从轮腿机器人传感器数据构建LQR状态
*/
int8_t LQR_BuildStateFromSensors(float position_x, float velocity_x,
float yaw_angle, float yaw_rate,
float left_leg_angle, float left_leg_rate,
float right_leg_angle, float right_leg_rate,
float body_pitch, float body_pitch_rate,
LQR_State_t *state) {
if (state == NULL) {
return -1;
}
state->s = position_x;
state->ds = velocity_x;
state->phi = yaw_angle;
state->dphi = yaw_rate;
state->theta_ll = left_leg_angle;
state->dtheta_ll = left_leg_rate;
state->theta_lr = right_leg_angle;
state->dtheta_lr = right_leg_rate;
state->theta_b = body_pitch;
state->dtheta_b = body_pitch_rate;
// 角度归一化
LQR_ANGLE_NORMALIZE(state->phi);
LQR_ANGLE_NORMALIZE(state->theta_ll);
LQR_ANGLE_NORMALIZE(state->theta_lr);
LQR_ANGLE_NORMALIZE(state->theta_b);
return 0;
}
/* Private functions -------------------------------------------------------- */
/**
* @brief 矩阵向量乘法: result = K * state_error
*
* K: 4x10矩阵
* state_error: 10x1向量
* result: 4x1向量
*/
static void LQR_MatrixMultiply(const float K[4][10], const float state_error[10], float result[4]) {
for (int i = 0; i < 4; i++) {
result[i] = 0.0f;
for (int j = 0; j < 10; j++) {
result[i] += K[i][j] * state_error[j];
}
}
}
/**
* @brief 计算状态误差(考虑角度周期性)
*/
static float LQR_ComputeStateError(float current, float reference) {
float error = current - reference;
// 对于角度状态，需要考虑周期性
// 这里假设大部分状态都是角度，如果需要区分可以添加参数
while (error > M_PI) error -= 2 * M_PI;
while (error < -M_PI) error += 2 * M_PI;
return error;
}

168
User/component/lqr.h
View File

@ -1,168 +0,0 @@
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ----------------------------------------------------------------- */
#include <stdint.h>
#include <stdbool.h>
#include <math.h>
#include "component/user_math.h"
/* Exported types ----------------------------------------------------------- */
/**
* @brief LQR控制器状态向量定义
*
* 状态向量维度: 10 x 1
* [s, ds, phi, dphi, theta_ll, dtheta_ll, theta_lr, dtheta_lr, theta_b, dtheta_b]^T
*/
typedef struct {
float s; // 机器人水平方向移动距离 (m)
float ds; // 机器人水平方向移动速度 (m/s)
float phi; // 机器人水平方向移动时yaw偏航角度 (rad)
float dphi; // yaw偏航角速度 (rad/s)
float theta_ll; // 左腿摆杆与竖直方向夹角 (rad)
float dtheta_ll; // 左腿摆杆角速度 (rad/s)
float theta_lr; // 右腿摆杆与竖直方向夹角 (rad)
float dtheta_lr; // 右腿摆杆角速度 (rad/s)
float theta_b; // 机体与水平方向夹角 (rad)
float dtheta_b; // 机体角速度 (rad/s)
} LQR_State_t;
/**
* @brief LQR控制器控制输入向量定义
*
* 控制向量维度: 4 x 1
* [T_wl, T_wr, T_bl, T_br]^T
*/
typedef struct {
float T_wl; // 左侧驱动轮输出力矩 (N*m)
float T_wr; // 右侧驱动轮输出力矩 (N*m)
float T_bl; // 左侧髋关节输出力矩 (N*m)
float T_br; // 右侧髋关节输出力矩 (N*m)
} LQR_Control_t;
/**
* @brief LQR增益矩阵K (4x10)
*/
typedef struct {
float K[4][10]; // LQR反馈增益矩阵
} LQR_GainMatrix_t;
/**
* @brief 简化的LQR控制器实例结构体
*/
typedef struct {
LQR_GainMatrix_t K; // 增益矩阵
LQR_State_t state; // 当前状态
LQR_State_t reference; // 参考状态
LQR_Control_t control; // 控制输出
float max_wheel_torque; // 轮毂电机最大力矩限制 (N*m)
float max_joint_torque; // 关节电机最大力矩限制 (N*m)
bool initialized; // 初始化标志
} LQR_Controller_t;
/* Exported constants ------------------------------------------------------- */
#define LQR_STATE_DIM (10) // 状态向量维度
#define LQR_CONTROL_DIM (4) // 控制向量维度
/* Exported macros ---------------------------------------------------------- */
/**
* @brief 角度归一化到[-PI, PI]
*/
#define LQR_ANGLE_NORMALIZE(angle) do { \
while((angle) > M_PI) (angle) -= 2*M_PI; \
while((angle) < -M_PI) (angle) += 2*M_PI; \
} while(0)
/**
* @brief 数值限幅
*/
#define LQR_CLAMP(val, min, max) ((val) < (min) ? (min) : ((val) > (max) ? (max) : (val)))
/* Exported functions prototypes -------------------------------------------- */
/**
* @brief 初始化LQR控制器
* @param lqr LQR控制器实例
* @param max_wheel_torque 轮毂电机最大力矩 (N*m)
* @param max_joint_torque 关节电机最大力矩 (N*m)
* @return 0:成功, -1:失败
*/
int8_t LQR_Init(LQR_Controller_t *lqr, float max_wheel_torque, float max_joint_torque);
/**
* @brief 设置固定LQR增益矩阵
* @param lqr LQR控制器实例
* @param K 增益矩阵
* @return 0:成功, -1:失败
*/
int8_t LQR_SetGainMatrix(LQR_Controller_t *lqr, const LQR_GainMatrix_t *K);
/**
* @brief 更新机器人状态
* @param lqr LQR控制器实例
* @param state 当前状态
* @return 0:成功, -1:失败
*/
int8_t LQR_UpdateState(LQR_Controller_t *lqr, const LQR_State_t *state);
/**
* @brief 设置参考状态
* @param lqr LQR控制器实例
* @param reference 参考状态
* @return 0:成功, -1:失败
*/
int8_t LQR_SetReference(LQR_Controller_t *lqr, const LQR_State_t *reference);
/**
* @brief 计算LQR控制输出
* @param lqr LQR控制器实例
* @return 0:成功, -1:失败
*/
int8_t LQR_ComputeControl(LQR_Controller_t *lqr);
/**
* @brief 获取控制输出
* @param lqr LQR控制器实例
* @param control 控制输出
* @return 0:成功, -1:失败
*/
int8_t LQR_GetControl(const LQR_Controller_t *lqr, LQR_Control_t *control);
/**
* @brief 重置LQR控制器状态
* @param lqr LQR控制器实例
*/
void LQR_Reset(LQR_Controller_t *lqr);
/**
* @brief 从轮腿机器人传感器数据构建LQR状态
* @param position_x 机体x位置 (m)
* @param velocity_x 机体x速度 (m/s)
* @param yaw_angle yaw角度 (rad)
* @param yaw_rate yaw角速度 (rad/s)
* @param left_leg_angle 左腿角度 (rad)
* @param left_leg_rate 左腿角速度 (rad/s)
* @param right_leg_angle 右腿角度 (rad)
* @param right_leg_rate 右腿角速度 (rad/s)
* @param body_pitch 机体pitch角度 (rad)
* @param body_pitch_rate 机体pitch角速度 (rad/s)
* @param state 输出状态
* @return 0:成功, -1:失败
*/
int8_t LQR_BuildStateFromSensors(float position_x, float velocity_x,
float yaw_angle, float yaw_rate,
float left_leg_angle, float left_leg_rate,
float right_leg_angle, float right_leg_rate,
float body_pitch, float body_pitch_rate,
LQR_State_t *state);
#ifdef __cplusplus
}
#endif

158
User/component/pid.c
View File

@ -1,158 +0,0 @@
/*
Modified from
https://github.com/PX4/Firmware/blob/master/src/lib/pid/pid.cpp
参考资料：
https://github.com/PX4/Firmware/issues/12362
https://dev.px4.io/master/en/flight_stack/controller_diagrams.html
https://docs.px4.io/master/en/config_mc/pid_tuning_guide_multicopter.html#standard_form
https://www.controleng.com/articles/not-all-pid-controllers-are-the-same/
https://en.wikipedia.org/wiki/PID_controller
http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-derivative-kick/
*/
#include "pid.h"
#define SIGMA 0.000001f
/**
* @brief 初始化PID
*
* @param pid PID结构体
* @param mode PID模式
* @param sample_freq 采样频率
* @param param PID参数
* @return int8_t 0对应没有错误
*/
int8_t PID_Init(KPID_t *pid, KPID_Mode_t mode, float sample_freq,
const KPID_Params_t *param) {
if (pid == NULL) return -1;
if (!isfinite(param->p)) return -1;
if (!isfinite(param->i)) return -1;
if (!isfinite(param->d)) return -1;
if (!isfinite(param->i_limit)) return -1;
if (!isfinite(param->out_limit)) return -1;
pid->param = param;
float dt_min = 1.0f / sample_freq;
if (isfinite(dt_min))
pid->dt_min = dt_min;
else
return -1;
LowPassFilter2p_Init(&(pid->dfilter), sample_freq, pid->param->d_cutoff_freq);
pid->mode = mode;
PID_Reset(pid);
return 0;
}
/**
* @brief PID计算
*
* @param pid PID结构体
* @param sp 设定值
* @param fb 反馈值
* @param fb_dot 反馈值微分
* @param dt 间隔时间
* @return float 计算的输出
*/
float PID_Calc(KPID_t *pid, float sp, float fb, float fb_dot, float dt) {
if (!isfinite(sp) || !isfinite(fb) || !isfinite(fb_dot) || !isfinite(dt)) {
return pid->last.out;
}
/* 计算误差值 */
const float err = CircleError(sp, fb, pid->param->range);
/* 计算P项 */
const float k_err = err * pid->param->k;
/* 计算D项 */
const float k_fb = pid->param->k * fb;
const float filtered_k_fb = LowPassFilter2p_Apply(&(pid->dfilter), k_fb);
float d;
switch (pid->mode) {
case KPID_MODE_CALC_D:
/* 通过fb计算D，避免了由于sp变化导致err突变的问题 */
/* 当sp不变时，err的微分等于负的fb的微分 */
d = (filtered_k_fb - pid->last.k_fb) / fmaxf(dt, pid->dt_min);
break;
case KPID_MODE_SET_D:
d = fb_dot;
break;
case KPID_MODE_NO_D:
d = 0.0f;
break;
}
pid->last.err = err;
pid->last.k_fb = filtered_k_fb;
if (!isfinite(d)) d = 0.0f;
/* 计算PD输出 */
float output = (k_err * pid->param->p) - (d * pid->param->d);
/* 计算I项 */
const float i = pid->i + (k_err * dt);
const float i_out = i * pid->param->i;
if (pid->param->i > SIGMA) {
/* 检查是否饱和 */
if (isfinite(i)) {
if ((fabsf(output + i_out) <= pid->param->out_limit) &&
(fabsf(i) <= pid->param->i_limit)) {
/* 未饱和，使用新积分 */
pid->i = i;
}
}
}
/* 计算PID输出 */
output += i_out;
/* 限制输出 */
if (isfinite(output)) {
if (pid->param->out_limit > SIGMA) {
output = AbsClip(output, pid->param->out_limit);
}
pid->last.out = output;
}
return pid->last.out;
}
/**
* @brief 重置微分项
*
* @param pid PID结构体
* @return int8_t 0对应没有错误
*/
int8_t PID_ResetIntegral(KPID_t *pid) {
if (pid == NULL) return -1;
pid->i = 0.0f;
return 0;
}
/**
* @brief 重置PID
*
* @param pid PID结构体
* @return int8_t 0对应没有错误
*/
int8_t PID_Reset(KPID_t *pid) {
if (pid == NULL) return -1;
pid->i = 0.0f;
pid->last.err = 0.0f;
pid->last.k_fb = 0.0f;
pid->last.out = 0.0f;
LowPassFilter2p_Reset(&(pid->dfilter), 0.0f);
return 0;
}

95
User/component/pid.h
View File

@ -1,95 +0,0 @@
/*
Modified from
https://github.com/PX4/Firmware/blob/master/src/lib/pid/pid.h
*/
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
#include <stdint.h>
#include "filter.h"
#include "user_math.h"
/* PID模式 */
typedef enum {
KPID_MODE_NO_D = 0, /* 不使用微分项，PI控制器 */
KPID_MODE_CALC_D, /* 根据反馈的值计算离散微分，忽略PID_Calc中的fb_dot */
KPID_MODE_SET_D /* 直接提供微分值，PID_Calc中的fb_dot将被使用，(Gyros) */
} KPID_Mode_t;
/* PID参数 */
typedef struct {
float k; /* 控制器增益，设置为1用于并行模式 */
float p; /* 比例项增益，设置为1用于标准形式 */
float i; /* 积分项增益 */
float d; /* 微分项增益 */
float i_limit; /* 积分项上限 */
float out_limit; /* 输出绝对值限制 */
float d_cutoff_freq; /* D项低通截止频率 */
float range; /* 计算循环误差时使用，大于0时启用 */
} KPID_Params_t;
/* PID主结构体 */
typedef struct {
KPID_Mode_t mode;
const KPID_Params_t *param;
float dt_min; /* 最小PID_Calc调用间隔 */
float i; /* 积分 */
struct {
float err; /* 上次误差 */
float k_fb; /* 上次反馈值 */
float out; /* 上次输出 */
} last;
LowPassFilter2p_t dfilter; /* D项低通滤波器 */
} KPID_t;
/**
* @brief 初始化PID
*
* @param pid PID结构体
* @param mode PID模式
* @param sample_freq 采样频率
* @param param PID参数
* @return int8_t 0对应没有错误
*/
int8_t PID_Init(KPID_t *pid, KPID_Mode_t mode, float sample_freq,
const KPID_Params_t *param);
/**
* @brief PID计算
*
* @param pid PID结构体
* @param sp 设定值
* @param fb 反馈值
* @param fb_dot 反馈值微分
* @param dt 间隔时间
* @return float 计算的输出
*/
float PID_Calc(KPID_t *pid, float sp, float fb, float fb_dot, float dt);
/**
* @brief 重置微分项
*
* @param pid PID结构体
* @return int8_t 0对应没有错误
*/
int8_t PID_ResetIntegral(KPID_t *pid);
/**
* @brief 重置PID
*
* @param pid PID结构体
* @return int8_t 0对应没有错误
*/
int8_t PID_Reset(KPID_t *pid);
#ifdef __cplusplus
}
#endif

132
User/component/user_math.c
View File

@ -1,132 +0,0 @@
/*
自定义的数学运算。
*/
#include "user_math.h"
#include <string.h>
inline float InvSqrt(float x) {
//#if 0
/* Fast inverse square-root */
/* See: http://en.wikipedia.org/wiki/Fast_inverse_square_root */
float halfx = 0.5f * x;
float y = x;
long i = *(long*)&y;
i = 0x5f3759df - (i>>1);
y = *(float*)&i;
y = y * (1.5f - (halfx * y * y));
y = y * (1.5f - (halfx * y * y));
return y;
//#else
// return 1.0f / sqrtf(x);
//#endif
}
inline float AbsClip(float in, float limit) {
return (in < -limit) ? -limit : ((in > limit) ? limit : in);
}
float fAbs(float in){
return (in > 0) ? in : -in;
}
inline void Clip(float *origin, float min, float max) {
if (*origin > max) *origin = max;
if (*origin < min) *origin = min;
}
inline float Sign(float in) { return (in > 0) ? 1.0f : 0.0f; }
/**
* \brief 将运动向量置零
*
* \param mv 被操作的值
*/
inline void ResetMoveVector(MoveVector_t *mv) { memset(mv, 0, sizeof(*mv)); }
/**
* \brief 计算循环值的误差，用于没有负数值，并在一定范围内变化的值
* 例如编码器：相差1.5PI其实等于相差-0.5PI
*
* \param sp 被操作的值
* \param fb 变化量
* \param range 被操作的值变化范围，正数时起效
*
* \return 函数运行结果
*/
inline float CircleError(float sp, float fb, float range) {
float error = sp - fb;
if (range > 0.0f) {
float half_range = range / 2.0f;
if (error > half_range)
error -= range;
else if (error < -half_range)
error += range;
}
return error;
}
/**
* \brief 循环加法，用于没有负数值，并在一定范围内变化的值
* 例如编码器，在0-2PI内变化，1.5PI + 1.5PI = 1PI
*
* \param origin 被操作的值
* \param delta 变化量
* \param range 被操作的值变化范围，正数时起效
*/
inline void CircleAdd(float *origin, float delta, float range) {
float out = *origin + delta;
if (range > 0.0f) {
if (out >= range)
out -= range;
else if (out < 0.0f)
out += range;
}
*origin = out;
}
/**
* @brief 循环值取反
*
* @param origin 被操作的值
*/
inline void CircleReverse(float *origin) { *origin = -(*origin) + M_2PI; }
/**
* @brief 根据目标弹丸速度计算摩擦轮转速
*
* @param bullet_speed 弹丸速度
* @param fric_radius 摩擦轮半径
* @param is17mm 是否为17mm
* @return 摩擦轮转速
*/
inline float CalculateRpm(float bullet_speed, float fric_radius, bool is17mm) {
if (bullet_speed == 0.0f) return 0.f;
if (is17mm) {
if (bullet_speed == 15.0f) return 4670.f;
if (bullet_speed == 18.0f) return 5200.f;
if (bullet_speed == 30.0f) return 7350.f;
} else {
if (bullet_speed == 10.0f) return 4450.f;
if (bullet_speed == 16.0f) return 5800.f;
}
/* 不为裁判系统设定值时,计算转速 */
return 60.0f * (float)bullet_speed / (M_2PI * fric_radius);
}
// /**
// * @brief 断言失败处理
// *
// * @param file 文件名
// * @param line 行号
// */
// void VerifyFailed(const char *file, uint32_t line) {
// UNUSED(file);
// UNUSED(line);
// while (1) {
// __NOP();
// }
// }

161
User/component/user_math.h
View File

@ -1,161 +0,0 @@
/*
自定义的数学运算。
*/
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
#include <float.h>
#include <math.h>
#include <stdbool.h>
#include <stdint.h>
#include <stddef.h>
#define M_DEG2RAD_MULT (0.01745329251f)
#define M_RAD2DEG_MULT (57.2957795131f)
#ifndef M_PI
#define M_PI 3.14159265358979323846f
#endif
#ifndef M_2PI
#define M_2PI 6.28318530717958647692f
#endif
#ifndef __packed
#define __packed __attribute__((__packed__))
#endif /* __packed */
#define max(a, b) \
({ \
__typeof__(a) _a = (a); \
__typeof__(b) _b = (b); \
_a > _b ? _a : _b; \
})
#define min(a, b) \
({ \
__typeof__(a) _a = (a); \
__typeof__(b) _b = (b); \
_a < _b ? _a : _b; \
})
/* 移动向量 */
typedef struct {
float vx; /* 前后平移 */
float vy; /* 左右平移 */
float wz; /* 转动 */
} MoveVector_t;
float InvSqrt(float x);
float AbsClip(float in, float limit);
float fAbs(float in);
void Clip(float *origin, float min, float max);
float Sign(float in);
/**
* \brief 将运动向量置零
*
* \param mv 被操作的值
*/
void ResetMoveVector(MoveVector_t *mv);
/**
* \brief 计算循环值的误差，用于没有负数值，并在一定范围内变化的值
* 例如编码器：相差1.5PI其实等于相差-0.5PI
*
* \param sp 被操作的值
* \param fb 变化量
* \param range 被操作的值变化范围，正数时起效
*
* \return 函数运行结果
*/
float CircleError(float sp, float fb, float range);
/**
* \brief 循环加法，用于没有负数值，并在一定范围内变化的值
* 例如编码器，在0-2PI内变化，1.5PI + 1.5PI = 1PI
*
* \param origin 被操作的值
* \param delta 变化量
* \param range 被操作的值变化范围，正数时起效
*/
void CircleAdd(float *origin, float delta, float range);
/**
* @brief 循环值取反
*
* @param origin 被操作的值
*/
void CircleReverse(float *origin);
/**
* @brief 根据目标弹丸速度计算摩擦轮转速
*
* @param bullet_speed 弹丸速度
* @param fric_radius 摩擦轮半径
* @param is17mm 是否为17mm
* @return 摩擦轮转速
*/
float CalculateRpm(float bullet_speed, float fric_radius, bool is17mm);
#ifdef __cplusplus
}
#endif
#ifdef DEBUG
/**
* @brief 如果表达式的值为假则运行处理函数
*
*/
#define ASSERT(expr) \
do { \
if (!(expr)) { \
VerifyFailed(__FILE__, __LINE__); \
} \
} while (0)
#else
/**
* @brief 未定DEBUG，表达式不会运行，断言被忽略
*
*/
#define ASSERT(expr) ((void)(0))
#endif
#ifdef DEBUG
/**
* @brief 如果表达式的值为假则运行处理函数
*
*/
#define VERIFY(expr) \
do { \
if (!(expr)) { \
VerifyFailed(__FILE__, __LINE__); \
} \
} while (0)
#else
/**
* @brief 表达式会运行，忽略表达式结果
*
*/
#define VERIFY(expr) ((void)(expr))
#endif
// /**
// * @brief 断言失败处理
// *
// * @param file 文件名
// * @param line 行号
// */
// void VerifyFailed(const char *file, uint32_t line);

381
User/component/vmc.c
View File

@ -1,381 +0,0 @@
/*
* VMC虚拟模型控制器实现
*
* 本文件实现了轮腿机器人的VMC (Virtual Model Control) 虚拟模型控制算法
* 主要功能包括:
* 1. 五连杆机构的正逆运动学解算
* 2. 虚拟力到关节力矩的映射
* 3. 地面接触检测
* 4. 等效摆动杆模型转换
*
* 参考文献:
* - 韭菜的菜 知乎: 平衡步兵控制系统设计
* - 上交轮腿电控开源方案 (2023)
*/
#include "vmc.h"
#include <string.h>
/* Private typedef ---------------------------------------------------------- */
/* Private define ----------------------------------------------------------- */
#define VMC_EPSILON (1e-6f) // 数值计算精度
#define VMC_MAX_ITER (10) // 最大迭代次数
/* Private macro ------------------------------------------------------------ */
/**
* @brief 限制数值范围
*/
#define VMC_CLAMP(val, min, max) ((val) < (min) ? (min) : ((val) > (max) ? (max) : (val)))
/**
* @brief 安全开方
*/
#define VMC_SAFE_SQRT(x) (((x) > 0) ? sqrtf(x) : 0.0f)
/* Private variables -------------------------------------------------------- */
/* Private function prototypes ---------------------------------------------- */
static int8_t VMC_ValidateParams(const VMC_Param_t *param);
static void VMC_UpdateKinematics(VMC_t *vmc, float phi1, float phi4);
static int8_t VMC_SolveClosedLoop(VMC_t *vmc);
static float VMC_ComputeNumericDerivative(float current, float previous, float dt);
/* Exported functions ------------------------------------------------------- */
/**
* @brief 初始化VMC控制器
*/
int8_t VMC_Init(VMC_t *vmc, const VMC_Param_t *param, float sample_freq) {
if (vmc == NULL || param == NULL || sample_freq <= 0) {
return -1;
}
// 复制参数
memcpy(&vmc->param, param, sizeof(VMC_Param_t));
// 设置控制周期
vmc->dt = 1.0f / sample_freq;
// 重置状态
VMC_Reset(vmc);
vmc->initialized = true;
return 0;
}
/**
* @brief VMC五连杆正解算
*
* 通过髋关节角度和机体姿态，计算足端位置和等效摆动杆参数
*
* 坐标系定义:
* - x轴: 机体前进方向为正
* - y轴: 竖直向下为正
* - 角度: 顺时针为正
*/
int8_t VMC_ForwardSolve(VMC_t *vmc, float phi1, float phi4, float body_pitch, float body_pitch_rate) {
if (vmc == NULL || !vmc->initialized) {
return -1;
}
VMC_Leg_t *leg = &vmc->leg;
// 保存历史值
leg->last_phi0 = leg->phi0;
leg->last_L0 = leg->L0;
leg->last_d_L0 = leg->d_L0;
leg->last_d_theta = leg->d_theta;
// 更新关节角度
leg->phi1 = phi1;
leg->phi4 = phi4;
// 更新运动学状态
VMC_UpdateKinematics(vmc, phi1, phi4);
// 求解闭环运动学
if (VMC_SolveClosedLoop(vmc) != 0) {
return -1;
}
// 计算足端坐标
leg->foot_x = leg->XC - vmc->param.hip_length / 2.0f;
leg->foot_y = leg->YC;
// 计算等效摆动杆参数
leg->L0 = VMC_SAFE_SQRT(leg->foot_x * leg->foot_x + leg->foot_y * leg->foot_y);
leg->phi0 = atan2f(leg->foot_y, leg->foot_x);
// 计算等效摆动杆角度(相对于机体坐标系)
leg->alpha = VMC_PI_2 - leg->phi0;
leg->theta = -(VMC_PI_2 + body_pitch - leg->phi0);
// 角度归一化
VMC_ANGLE_NORMALIZE(leg->theta);
VMC_ANGLE_NORMALIZE(leg->phi0);
// 计算角速度和长度变化率
leg->d_phi0 = VMC_ComputeNumericDerivative(leg->phi0, leg->last_phi0, vmc->dt);
leg->d_alpha = -leg->d_phi0;
leg->d_theta = body_pitch_rate + leg->d_phi0;
leg->d_L0 = VMC_ComputeNumericDerivative(leg->L0, leg->last_L0, vmc->dt);
// 计算角加速度
leg->dd_theta = VMC_ComputeNumericDerivative(leg->d_theta, leg->last_d_theta, vmc->dt);
return 0;
}
/**
* @brief VMC五连杆逆解算(力矩分配)
*
* 根据期望的虚拟力和力矩，通过雅可比矩阵计算关节力矩
*/
int8_t VMC_InverseSolve(VMC_t *vmc, float F_virtual, float T_virtual) {
if (vmc == NULL || !vmc->initialized) {
return -1;
}
// 保存虚拟力和力矩
vmc->leg.F_virtual = -F_virtual;
vmc->leg.T_virtual = T_virtual;
// 计算雅可比矩阵
if (VMC_ComputeJacobian(vmc) != 0) {
return -1;
}
VMC_Leg_t *leg = &vmc->leg;
// 通过雅可比转置计算关节力矩
// tau = J^T * F_virtual
leg->tau_hip_rear = leg->J11 * vmc->leg.F_virtual + leg->J12 * vmc->leg.T_virtual;
leg->tau_hip_front = leg->J21 * vmc->leg.F_virtual + leg->J22 * vmc->leg.T_virtual;
return 0;
}
/**
* @brief 地面接触检测
*
* 基于虚拟力和腿部状态估计地面法向力
*/
float VMC_GroundContactDetection(VMC_t *vmc) {
if (vmc == NULL || !vmc->initialized) {
return 0.0f;
}
VMC_Leg_t *leg = &vmc->leg;
// 计算地面法向力
// Fn = F0*cos(theta) + Tp*sin(theta)/L0 + mg
leg->Fn = leg->F_virtual * cosf(leg->theta) +
leg->T_virtual * sinf(leg->theta) / leg->L0 +
vmc->param.wheel_mass * 9.8f; // 添加轮子重力
// 地面接触判断
leg->is_ground_contact = (leg->Fn > 10.0f); // 10N阈值
return leg->Fn;
}
/**
* @brief 获取足端位置
*/
int8_t VMC_GetFootPosition(const VMC_t *vmc, float *x, float *y) {
if (vmc == NULL || !vmc->initialized || x == NULL || y == NULL) {
return -1;
}
*x = vmc->leg.foot_x;
*y = vmc->leg.foot_y;
return 0;
}
/**
* @brief 获取等效摆动杆参数
*/
int8_t VMC_GetVirtualLegState(const VMC_t *vmc, float *length, float *angle, float *d_length, float *d_angle) {
if (vmc == NULL || !vmc->initialized) {
return -1;
}
if (length) *length = vmc->leg.L0;
if (angle) *angle = vmc->leg.theta;
if (d_length) *d_length = vmc->leg.d_L0;
if (d_angle) *d_angle = vmc->leg.d_theta;
return 0;
}
/**
* @brief 获取关节输出力矩
*/
int8_t VMC_GetJointTorques(const VMC_t *vmc, float *tau_front, float *tau_rear) {
if (vmc == NULL || !vmc->initialized || tau_front == NULL || tau_rear == NULL) {
return -1;
}
*tau_front = vmc->leg.tau_hip_front;
*tau_rear = vmc->leg.tau_hip_rear;
return 0;
}
/**
* @brief 重置VMC控制器状态
*/
void VMC_Reset(VMC_t *vmc) {
if (vmc == NULL) {
return;
}
// 清零腿部状态
memset(&vmc->leg, 0, sizeof(VMC_Leg_t));
// 设置初始值
vmc->leg.L0 = 0.15f; // 默认腿长15cm
vmc->leg.theta = 0.0f;
}
/**
* @brief 设置虚拟力和力矩
*/
void VMC_SetVirtualForces(VMC_t *vmc, float F_virtual, float T_virtual) {
if (vmc == NULL || !vmc->initialized) {
return;
}
vmc->leg.F_virtual = F_virtual;
vmc->leg.T_virtual = T_virtual;
}
/**
* @brief 计算雅可比矩阵
*
* 根据当前关节配置计算从虚拟力到关节力矩的雅可比矩阵
*/
int8_t VMC_ComputeJacobian(VMC_t *vmc) {
if (vmc == NULL || !vmc->initialized) {
return -1;
}
VMC_Leg_t *leg = &vmc->leg;
// 检查分母不为零
float sin_diff = sinf(leg->phi3 - leg->phi2);
if (fabsf(sin_diff) < VMC_EPSILON) {
return -1; // 奇异配置
}
// 计算雅可比矩阵元素
// J11: 后髋关节到支撑力的雅可比
leg->J11 = (vmc->param.leg_1 * sinf(leg->phi0 - leg->phi3) *
sinf(leg->phi1 - leg->phi2)) / sin_diff;
// J12: 后髋关节到摆动力矩的雅可比
leg->J12 = (vmc->param.leg_1 * cosf(leg->phi0 - leg->phi3) *
sinf(leg->phi1 - leg->phi2)) / (leg->L0 * sin_diff);
// J21: 前髋关节到支撑力的雅可比
leg->J21 = (vmc->param.leg_4 * sinf(leg->phi0 - leg->phi2) *
sinf(leg->phi3 - leg->phi4)) / sin_diff;
// J22: 前髋关节到摆动力矩的雅可比
leg->J22 = (vmc->param.leg_4 * cosf(leg->phi0 - leg->phi2) *
sinf(leg->phi3 - leg->phi4)) / (leg->L0 * sin_diff);
return 0;
}
/* Private functions -------------------------------------------------------- */
/**
* @brief 验证VMC参数有效性
*/
static int8_t VMC_ValidateParams(const VMC_Param_t *param) {
if (param->hip_length <= 0 || param->leg_1 <= 0 || param->leg_2 <= 0 ||
param->leg_3 <= 0 || param->leg_4 <= 0 || param->wheel_radius <= 0) {
return -1;
}
// 检查腿部几何约束
if (param->leg_2 + param->leg_3 <= param->leg_1 + param->leg_4) {
return -1; // 不满足闭环几何约束
}
return 0;
}
/**
* @brief 更新基本运动学参数
*/
static void VMC_UpdateKinematics(VMC_t *vmc, float phi1, float phi4) {
VMC_Leg_t *leg = &vmc->leg;
// 计算关键点坐标
// 点B (后髋关节末端)
leg->XB = vmc->param.leg_1 * cosf(phi1);
leg->YB = vmc->param.leg_1 * sinf(phi1);
// 点D (前髋关节末端)
leg->XD = vmc->param.hip_length + vmc->param.leg_4 * cosf(phi4);
leg->YD = vmc->param.leg_4 * sinf(phi4);
// 计算BD连杆长度
float dx = leg->XD - leg->XB;
float dy = leg->YD - leg->YB;
leg->lBD = VMC_SAFE_SQRT(dx * dx + dy * dy);
}
/**
* @brief 求解闭环运动学方程
*
* 根据两个髋关节角度，求解中间关节角度
*/
static int8_t VMC_SolveClosedLoop(VMC_t *vmc) {
VMC_Leg_t *leg = &vmc->leg;
// 使用余弦定理求解phi2
leg->A0 = 2 * vmc->param.leg_2 * (leg->XD - leg->XB);
leg->B0 = 2 * vmc->param.leg_2 * (leg->YD - leg->YB);
leg->C0 = vmc->param.leg_2 * vmc->param.leg_2 +
leg->lBD * leg->lBD -
vmc->param.leg_3 * vmc->param.leg_3;
// 检查判别式
float discriminant = leg->A0 * leg->A0 + leg->B0 * leg->B0 - leg->C0 * leg->C0;
if (discriminant < 0) {
return -1; // 无解
}
float sqrt_discriminant = VMC_SAFE_SQRT(discriminant);
// 计算phi2 (选择合适的解)
leg->phi2 = 2 * atan2f(leg->B0 + sqrt_discriminant, leg->A0 + leg->C0);
// 计算phi3
leg->phi3 = atan2f(leg->YB - leg->YD + vmc->param.leg_2 * sinf(leg->phi2),
leg->XB - leg->XD + vmc->param.leg_2 * cosf(leg->phi2));
// 计算足端坐标点C
leg->XC = leg->XB + vmc->param.leg_2 * cosf(leg->phi2);
leg->YC = leg->YB + vmc->param.leg_2 * sinf(leg->phi2);
return 0;
}
/**
* @brief 计算数值微分
*/
static float VMC_ComputeNumericDerivative(float current, float previous, float dt) {
if (dt <= 0) {
return 0.0f;
}
return (current - previous) / dt;
}

196
User/component/vmc.h
View File

@ -1,196 +0,0 @@
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ----------------------------------------------------------------- */
#include <stdint.h>
#include <stdbool.h>
#include <math.h>
#include "kinematics.h"
/* Exported types ----------------------------------------------------------- */
/**
* @brief VMC虚拟模型控制参数结构体
*/
typedef struct {
float hip_length; // 髋关节间距
float leg_1; // 大腿前端长度 (L1)
float leg_2; // 大腿后端长度 (L2)
float leg_3; // 小腿长度 (L3)
float leg_4; // 小腿前端长度 (L4)
float wheel_radius; // 轮子半径
float wheel_mass; // 轮子质量
} VMC_Param_t;
/**
* @brief VMC腿部运动学状态结构体
*/
typedef struct {
// 关节角度
float phi1; // 后髋关节角度 (rad)
float phi2; // 大腿后端角度 (rad)
float phi3; // 小腿角度 (rad)
float phi4; // 前髋关节角度 (rad)
// 足端坐标
float foot_x; // 足端x坐标
float foot_y; // 足端y坐标
// 等效摆动杆参数
float L0; // 等效摆动杆长度
float d_L0; // 等效摆动杆长度变化率
float theta; // 等效摆动杆角度
float d_theta; // 等效摆动杆角速度
float dd_theta; // 等效摆动杆角加速度
// 虚拟力和力矩
float F_virtual; // 虚拟支撑力
float T_virtual; // 虚拟摆动力矩
// 雅可比矩阵元素
float J11, J12, J21, J22;
// 输出力矩
float tau_hip_front; // 前髋关节输出力矩
float tau_hip_rear; // 后髋关节输出力矩
// 内部计算变量
float XB, YB, XC, YC, XD, YD; // 各关键点坐标
float lBD; // BD连杆长度
float A0, B0, C0; // 运动学计算中间变量
float phi0; // 足端极角
float alpha; // 等效摆动杆与竖直方向夹角
float d_phi0; // 足端极角变化率
float d_alpha; // alpha角变化率
// 历史值（用于数值微分）
float last_phi0;
float last_L0;
float last_d_L0;
float last_d_theta;
// 地面接触检测
float Fn; // 地面法向力
bool is_ground_contact; // 地面接触标志
} VMC_Leg_t;
/**
* @brief VMC控制器结构体
*/
typedef struct {
VMC_Param_t param; // VMC参数
VMC_Leg_t leg; // 腿部状态
float dt; // 控制周期
bool initialized; // 初始化标志
} VMC_t;
/* Exported constants ------------------------------------------------------- */
#define VMC_PI_2 (1.5707963267948966f)
#define VMC_PI (3.1415926535897932f)
#define VMC_2PI (6.2831853071795865f)
/* Exported macros ---------------------------------------------------------- */
/**
* @brief 角度范围限制到[-PI, PI]
*/
#define VMC_ANGLE_NORMALIZE(angle) do { \
while((angle) > VMC_PI) (angle) -= VMC_2PI; \
while((angle) < -VMC_PI) (angle) += VMC_2PI; \
} while(0)
/* Exported functions prototypes -------------------------------------------- */
/**
* @brief 初始化VMC控制器
* @param vmc VMC控制器实例
* @param param VMC参数
* @param sample_freq 采样频率 (Hz)
* @return 0:成功, -1:失败
*/
int8_t VMC_Init(VMC_t *vmc, const VMC_Param_t *param, float sample_freq);
/**
* @brief VMC五连杆正解算
* @param vmc VMC控制器实例
* @param phi1 后髋关节角度 (rad)
* @param phi4 前髋关节角度 (rad)
* @param body_pitch 机体pitch角 (rad)
* @param body_pitch_rate 机体pitch角速度 (rad/s)
* @return 0:成功, -1:失败
*/
int8_t VMC_ForwardSolve(VMC_t *vmc, float phi1, float phi4, float body_pitch, float body_pitch_rate);
/**
* @brief VMC五连杆逆解算(力矩分配)
* @param vmc VMC控制器实例
* @param F_virtual 期望虚拟支撑力 (N)
* @param T_virtual 期望虚拟摆动力矩 (N*m)
* @return 0:成功, -1:失败
*/
int8_t VMC_InverseSolve(VMC_t *vmc, float F_virtual, float T_virtual);
/**
* @brief 地面接触检测
* @param vmc VMC控制器实例
* @return 地面法向力 (N)
*/
float VMC_GroundContactDetection(VMC_t *vmc);
/**
* @brief 获取足端位置(直角坐标)
* @param vmc VMC控制器实例
* @param x 足端x坐标输出
* @param y 足端y坐标输出
* @return 0:成功, -1:失败
*/
int8_t VMC_GetFootPosition(const VMC_t *vmc, float *x, float *y);
/**
* @brief 获取等效摆动杆参数
* @param vmc VMC控制器实例
* @param length 等效摆动杆长度输出
* @param angle 等效摆动杆角度输出
* @param d_length 等效摆动杆长度变化率输出
* @param d_angle 等效摆动杆角速度输出
* @return 0:成功, -1:失败
*/
int8_t VMC_GetVirtualLegState(const VMC_t *vmc, float *length, float *angle, float *d_length, float *d_angle);
/**
* @brief 获取关节输出力矩
* @param vmc VMC控制器实例
* @param tau_front 前髋关节力矩输出
* @param tau_rear 后髋关节力矩输出
* @return 0:成功, -1:失败
*/
int8_t VMC_GetJointTorques(const VMC_t *vmc, float *tau_front, float *tau_rear);
/**
* @brief 重置VMC控制器状态
* @param vmc VMC控制器实例
*/
void VMC_Reset(VMC_t *vmc);
/**
* @brief 设置虚拟力和力矩
* @param vmc VMC控制器实例
* @param F_virtual 虚拟支撑力 (N)
* @param T_virtual 虚拟摆动力矩 (N*m)
*/
void VMC_SetVirtualForces(VMC_t *vmc, float F_virtual, float T_virtual);
/**
* @brief 计算雅可比矩阵
* @param vmc VMC控制器实例
* @return 0:成功, -1:失败
*/
int8_t VMC_ComputeJacobian(VMC_t *vmc);
#ifdef __cplusplus
}
#endif

44
User/device/buzzer.c
View File

@ -1,44 +0,0 @@
#include "device/buzzer.h"
int8_t BUZZER_Init(BUZZER_t *buzzer, BSP_PWM_Channel_t channel) {
if (buzzer == NULL) return DEVICE_ERR;
buzzer->channel = channel;
buzzer->header.online = true;
BUZZER_Stop(buzzer);
return DEVICE_OK ;
}
int8_t BUZZER_Start(BUZZER_t *buzzer) {
if (buzzer == NULL || !buzzer->header.online)
return DEVICE_ERR;
return (BSP_PWM_Start(buzzer->channel) == BSP_OK) ?
DEVICE_OK : DEVICE_ERR;
}
int8_t BUZZER_Stop(BUZZER_t *buzzer) {
if (buzzer == NULL || !buzzer->header.online)
return DEVICE_ERR;
return (BSP_PWM_Stop(buzzer->channel) == BSP_OK) ?
DEVICE_OK : DEVICE_ERR;
}
int8_t BUZZER_Set(BUZZER_t *buzzer, float freq, float duty_cycle) {
if (buzzer == NULL || !buzzer->header.online)
return DEVICE_ERR;
int result = DEVICE_OK ;
if (BSP_PWM_SetFreq(buzzer->channel, freq) != BSP_OK)
result = DEVICE_ERR;
if (BSP_PWM_SetComp(buzzer->channel, duty_cycle) != BSP_OK)
result = DEVICE_ERR;
return result;
}

35
User/device/buzzer.h
View File

@ -1,35 +0,0 @@
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ----------------------------------------------------------------- */
#include "device.h"
#include "bsp/pwm.h"
#include <stddef.h>
/* Exported constants ------------------------------------------------------- */
/* Exported types ----------------------------------------------------------- */
typedef struct {
DEVICE_Header_t header;
BSP_PWM_Channel_t channel;
} BUZZER_t;
/* Exported functions prototypes -------------------------------------------- */
int8_t BUZZER_Init(BUZZER_t *buzzer, BSP_PWM_Channel_t channel);
int8_t BUZZER_Start(BUZZER_t *buzzer);
int8_t BUZZER_Stop(BUZZER_t *buzzer);
int8_t BUZZER_Set(BUZZER_t *buzzer, float freq, float duty_cycle);
#ifdef __cplusplus
}
#endif

31
User/device/device.h
View File

@ -1,31 +0,0 @@
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
#include <stdbool.h>
#include <stdint.h>
#define DEVICE_OK (0)
#define DEVICE_ERR (-1)
#define DEVICE_ERR_NULL (-2)
#define DEVICE_ERR_INITED (-3)
#define DEVICE_ERR_NO_DEV (-4)
/* AUTO GENERATED SIGNALS BEGIN */
#define SIGNAL_DR16_RAW_REDY (1u << 0)
/* AUTO GENERATED SIGNALS END */
/* USER SIGNALS BEGIN */
/* USER SIGNALS END */
/*设备层通用Header*/
typedef struct {
bool online;
uint64_t last_online_time;
} DEVICE_Header_t;
#ifdef __cplusplus
}
#endif

19
User/device/device_config.yaml
View File

@ -1,19 +0,0 @@
buzzer:
bsp_config: {}
enabled: true
dm_imu:
bsp_config: {}
enabled: true
dr16:
bsp_config:
BSP_UART_DR16: BSP_UART_DR16
enabled: true
motor:
bsp_config: {}
enabled: true
motor_lk:
bsp_config: {}
enabled: true
motor_lz:
bsp_config: {}
enabled: true

271
User/device/dm_imu.c
View File

@ -1,271 +0,0 @@
/*
DM_IMU数据获取
*/
/* Includes ----------------------------------------------------------------- */
#include "dm_imu.h"
#include "bsp/can.h"
#include "bsp/time.h"
#include "component/user_math.h"
#include <string.h>
/* Private define ----------------------------------------------------------- */
#define DM_IMU_OFFLINE_TIMEOUT 1000 // 设备离线判定时间1000ms
#define ACCEL_CAN_MAX (58.8f)
#define ACCEL_CAN_MIN (-58.8f)
#define GYRO_CAN_MAX (34.88f)
#define GYRO_CAN_MIN (-34.88f)
#define PITCH_CAN_MAX (90.0f)
#define PITCH_CAN_MIN (-90.0f)
#define ROLL_CAN_MAX (180.0f)
#define ROLL_CAN_MIN (-180.0f)
#define YAW_CAN_MAX (180.0f)
#define YAW_CAN_MIN (-180.0f)
#define TEMP_MIN (0.0f)
#define TEMP_MAX (60.0f)
#define Quaternion_MIN (-1.0f)
#define Quaternion_MAX (1.0f)
/* Private macro ------------------------------------------------------------ */
/* Private typedef ---------------------------------------------------------- */
/* Private variables -------------------------------------------------------- */
/* Private function --------------------------------------------------------- */
static uint8_t count = 0; // 计数器，用于判断设备是否离线
/**
* @brief: 无符号整数转换为浮点数函数
*/
static float uint_to_float(int x_int, float x_min, float x_max, int bits)
{
float span = x_max - x_min;
float offset = x_min;
return ((float)x_int)*span/((float)((1<<bits)-1)) + offset;
}
/**
* @brief 解析加速度计数据
*/
static int8_t DM_IMU_ParseAccelData(DM_IMU_t *imu, uint8_t *data, uint8_t len) {
if (imu == NULL || data == NULL || len < 8) {
return DEVICE_ERR;
}
int8_t temp = data[1];
uint16_t acc_x_raw = (data[3] << 8) | data[2];
uint16_t acc_y_raw = (data[5] << 8) | data[4];
uint16_t acc_z_raw = (data[7] << 8) | data[6];
imu->data.temp = (float)temp;
imu->data.accl.x = uint_to_float(acc_x_raw, ACCEL_CAN_MIN, ACCEL_CAN_MAX, 16);
imu->data.accl.y = uint_to_float(acc_y_raw, ACCEL_CAN_MIN, ACCEL_CAN_MAX, 16);
imu->data.accl.z = uint_to_float(acc_z_raw, ACCEL_CAN_MIN, ACCEL_CAN_MAX, 16);
return DEVICE_OK;
}
/**
* @brief 解析陀螺仪数据
*/
static int8_t DM_IMU_ParseGyroData(DM_IMU_t *imu, uint8_t *data, uint8_t len) {
if (imu == NULL || data == NULL || len < 8) {
return DEVICE_ERR;
}
uint16_t gyro_x_raw = (data[3] << 8) | data[2];
uint16_t gyro_y_raw = (data[5] << 8) | data[4];
uint16_t gyro_z_raw = (data[7] << 8) | data[6];
imu->data.gyro.x = uint_to_float(gyro_x_raw, GYRO_CAN_MIN, GYRO_CAN_MAX, 16);
imu->data.gyro.y = uint_to_float(gyro_y_raw, GYRO_CAN_MIN, GYRO_CAN_MAX, 16);
imu->data.gyro.z = uint_to_float(gyro_z_raw, GYRO_CAN_MIN, GYRO_CAN_MAX, 16);
return DEVICE_OK;
}
/**
* @brief 解析欧拉角数据
*/
static int8_t DM_IMU_ParseEulerData(DM_IMU_t *imu, uint8_t *data, uint8_t len) {
if (imu == NULL || data == NULL || len < 8) {
return DEVICE_ERR;
}
uint16_t pit_raw = (data[3] << 8) | data[2];
uint16_t yaw_raw = (data[5] << 8) | data[4];
uint16_t rol_raw = (data[7] << 8) | data[6];
imu->data.euler.pit = uint_to_float(pit_raw, PITCH_CAN_MIN, PITCH_CAN_MAX, 16) * M_DEG2RAD_MULT;
imu->data.euler.yaw = uint_to_float(yaw_raw, YAW_CAN_MIN, YAW_CAN_MAX, 16) * M_DEG2RAD_MULT;
imu->data.euler.rol = uint_to_float(rol_raw, ROLL_CAN_MIN, ROLL_CAN_MAX, 16) * M_DEG2RAD_MULT;
return DEVICE_OK;
}
/**
* @brief 解析四元数数据
*/
static int8_t DM_IMU_ParseQuaternionData(DM_IMU_t *imu, uint8_t *data, uint8_t len) {
if (imu == NULL || data == NULL || len < 8) {
return DEVICE_ERR;
}
int w = (data[1] << 6) | ((data[2] & 0xF8) >> 2);
int x = ((data[2] & 0x03) << 12) | (data[3] << 4) | ((data[4] & 0xF0) >> 4);
int y = ((data[4] & 0x0F) << 10) | (data[5] << 2) | ((data[6] & 0xC0) >> 6);
int z = ((data[6] & 0x3F) << 8) | data[7];
imu->data.quat.q0 = uint_to_float(w, Quaternion_MIN, Quaternion_MAX, 14);
imu->data.quat.q1 = uint_to_float(x, Quaternion_MIN, Quaternion_MAX, 14);
imu->data.quat.q2 = uint_to_float(y, Quaternion_MIN, Quaternion_MAX, 14);
imu->data.quat.q3 = uint_to_float(z, Quaternion_MIN, Quaternion_MAX, 14);
return DEVICE_OK;
}
/* Exported functions ------------------------------------------------------- */
/**
* @brief 初始化DM IMU设备
*/
int8_t DM_IMU_Init(DM_IMU_t *imu, DM_IMU_Param_t *param) {
if (imu == NULL || param == NULL) {
return DEVICE_ERR_NULL;
}
// 初始化设备头部
imu->header.online = false;
imu->header.last_online_time = 0;
// 配置参数
imu->param.can = param->can;
imu->param.can_id = param->can_id;
imu->param.device_id = param->device_id;
imu->param.master_id = param->master_id;
// 清零数据
memset(&imu->data, 0, sizeof(DM_IMU_Data_t));
// 注册CAN接收队列，用于接收回复报文
int8_t result = BSP_CAN_RegisterId(imu->param.can, imu->param.master_id, 10);
if (result != BSP_OK) {
return DEVICE_ERR;
}
return DEVICE_OK;
}
/**
* @brief 请求IMU数据
*/
int8_t DM_IMU_Request(DM_IMU_t *imu, DM_IMU_RID_t rid) {
if (imu == NULL) {
return DEVICE_ERR_NULL;
}
// 构造发送数据：id_L, id_H(DM_IMU_ID), RID, 0xcc
uint8_t tx_data[4] = {
imu->param.device_id & 0xFF, // id_L
(imu->param.device_id >> 8) & 0xFF, // id_H
(uint8_t)rid, // RID
0xCC // 固定值
};
// 发送标准数据帧
BSP_CAN_StdDataFrame_t frame = {
.id = imu->param.can_id,
.dlc = 4,
};
memcpy(frame.data, tx_data, 4);
int8_t result = BSP_CAN_TransmitStdDataFrame(imu->param.can, &frame);
return (result == BSP_OK) ? DEVICE_OK : DEVICE_ERR;
}
/**
* @brief 更新IMU数据（从CAN中获取所有数据并解析）
*/
int8_t DM_IMU_Update(DM_IMU_t *imu) {
if (imu == NULL) {
return DEVICE_ERR_NULL;
}
BSP_CAN_Message_t msg;
int8_t result;
bool data_received = false;
// 持续接收所有可用消息
while ((result = BSP_CAN_GetMessage(imu->param.can, imu->param.master_id, &msg, BSP_CAN_TIMEOUT_IMMEDIATE)) == BSP_OK) {
// 验证回复数据格式（至少检查数据长度）
if (msg.dlc < 3) {
continue; // 跳过无效消息
}
// 根据数据位的第0位确定反馈报文类型
uint8_t rid = msg.data[0] & 0x0F; // 取第0位的低4位作为RID
// 根据RID类型解析数据
int8_t parse_result = DEVICE_ERR;
switch (rid) {
case 0x01: // RID_ACCL
parse_result = DM_IMU_ParseAccelData(imu, msg.data, msg.dlc);
break;
case 0x02: // RID_GYRO
parse_result = DM_IMU_ParseGyroData(imu, msg.data, msg.dlc);
break;
case 0x03: // RID_EULER
parse_result = DM_IMU_ParseEulerData(imu, msg.data, msg.dlc);
break;
case 0x04: // RID_QUATERNION
parse_result = DM_IMU_ParseQuaternionData(imu, msg.data, msg.dlc);
break;
default:
continue; // 跳过未知类型的消息
}
// 如果解析成功，标记为收到数据
if (parse_result == DEVICE_OK) {
data_received = true;
}
}
// 如果收到任何有效数据，更新设备状态
if (data_received) {
imu->header.online = true;
imu->header.last_online_time = BSP_TIME_Get_ms();
return DEVICE_OK;
}
return DEVICE_ERR;
}
/**
* @brief 自动更新IMU所有数据（包括加速度计、陀螺仪、欧拉角和四元数,最高1khz）
*/
int8_t DM_IMU_AutoUpdateAll(DM_IMU_t *imu){
if (imu == NULL) {
return DEVICE_ERR_NULL;
}
switch (count) {
case 0:
DM_IMU_Request(imu, RID_ACCL);
break;
case 1:
DM_IMU_Request(imu, RID_GYRO);
break;
case 2:
DM_IMU_Request(imu, RID_EULER);
break;
case 3:
DM_IMU_Request(imu, RID_QUATERNION);
DM_IMU_Update(imu); // 更新所有数据
break;
}
count++;
if (count >= 4) {
count = 0; // 重置计数器
}
return DEVICE_OK;
}
/**
* @brief 检查设备是否在线
*/
bool DM_IMU_IsOnline(DM_IMU_t *imu) {
if (imu == NULL) {
return false;
}
uint32_t current_time = BSP_TIME_Get_ms();
return imu->header.online &&
(current_time - imu->header.last_online_time < DM_IMU_OFFLINE_TIMEOUT);
}

90
User/device/dm_imu.h
View File

@ -1,90 +0,0 @@
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ----------------------------------------------------------------- */
#include "device/device.h"
#include "component/ahrs.h"
#include "bsp/can.h"
/* Exported constants ------------------------------------------------------- */
#define DM_IMU_CAN_ID_DEFAULT 0x6FF
#define DM_IMU_ID_DEFAULT 0x42
#define DM_IMU_MST_ID_DEFAULT 0x43
/* Exported macro ----------------------------------------------------------- */
/* Exported types ----------------------------------------------------------- */
typedef struct {
BSP_CAN_t can; // CAN总线句柄
uint16_t can_id; // CAN通信ID
uint8_t device_id; // 设备ID
uint8_t master_id; // 主机ID
} DM_IMU_Param_t;
typedef enum {
RID_ACCL = 0x01, // 加速度计
RID_GYRO = 0x02, // 陀螺仪
RID_EULER = 0x03, // 欧拉角
RID_QUATERNION = 0x04, // 四元数
} DM_IMU_RID_t;
typedef struct {
AHRS_Accl_t accl; // 加速度计
AHRS_Gyro_t gyro; // 陀螺仪
AHRS_Eulr_t euler; // 欧拉角
AHRS_Quaternion_t quat; // 四元数
float temp; // 温度
} DM_IMU_Data_t;
typedef struct {
DEVICE_Header_t header;
DM_IMU_Param_t param; // IMU参数配置
DM_IMU_Data_t data; // IMU数据
} DM_IMU_t;
/* Exported functions prototypes -------------------------------------------- */
/**
* @brief 初始化DM IMU设备
* @param imu DM IMU设备结构体指针
* @param param IMU参数配置指针
* @return DEVICE_OK 成功，其他值失败
*/
int8_t DM_IMU_Init(DM_IMU_t *imu, DM_IMU_Param_t *param);
/**
* @brief 请求IMU数据
* @param imu DM IMU设备结构体指针
* @param rid 请求的数据类型
* @return DEVICE_OK 成功，其他值失败
*/
int8_t DM_IMU_Request(DM_IMU_t *imu, DM_IMU_RID_t rid);
/**
* @brief 更新IMU数据（从CAN中获取所有数据并解析）
* @param imu DM IMU设备结构体指针
* @return DEVICE_OK 成功，其他值失败
*/
int8_t DM_IMU_Update(DM_IMU_t *imu);
/**
* @brief 自动更新IMU所有数据（包括加速度计、陀螺仪、欧拉角和四元数,最高1khz，运行4次才有完整数据）
* @param imu DM IMU设备结构体指针
* @return DEVICE_OK 成功，其他值失败
*/
int8_t DM_IMU_AutoUpdateAll(DM_IMU_t *imu);
/**
* @brief 检查设备是否在线
* @param imu DM IMU设备结构体指针
* @return true 在线，false 离线
*/
bool DM_IMU_IsOnline(DM_IMU_t *imu);
#ifdef __cplusplus
}
#endif

85
User/device/dr16.c
View File

@ -1,85 +0,0 @@
/*
DR16接收机
*/
/* Includes ----------------------------------------------------------------- */
#include "dr16.h"
#include <string.h>
#include "bsp/uart.h"
#include "bsp/time.h"
/* Private define ----------------------------------------------------------- */
#define DR16_CH_VALUE_MIN (364u)
#define DR16_CH_VALUE_MID (1024u)
#define DR16_CH_VALUE_MAX (1684u)
/* Private macro ------------------------------------------------------------ */
/* Private typedef ---------------------------------------------------------- */
/* Private variables -------------------------------------------------------- */
static osThreadId_t thread_alert;
static bool inited = false;
/* Private function -------------------------------------------------------- */
static void DR16_RxCpltCallback(void) {
osThreadFlagsSet(thread_alert, SIGNAL_DR16_RAW_REDY);
}
static bool DR16_DataCorrupted(const DR16_t *dr16) {
if (dr16 == NULL) return DEVICE_ERR_NULL;
if ((dr16->data.ch_r_x < DR16_CH_VALUE_MIN) ||
(dr16->data.ch_r_x > DR16_CH_VALUE_MAX))
return true;
if ((dr16->data.ch_r_y < DR16_CH_VALUE_MIN) ||
(dr16->data.ch_r_y > DR16_CH_VALUE_MAX))
return true;
if ((dr16->data.ch_l_x < DR16_CH_VALUE_MIN) ||
(dr16->data.ch_l_x > DR16_CH_VALUE_MAX))
return true;
if ((dr16->data.ch_l_y < DR16_CH_VALUE_MIN) ||
(dr16->data.ch_l_y > DR16_CH_VALUE_MAX))
return true;
if (dr16->data.sw_l == 0) return true;
if (dr16->data.sw_r == 0) return true;
return false;
}
/* Exported functions ------------------------------------------------------- */
int8_t DR16_Init(DR16_t *dr16) {
if (dr16 == NULL) return DEVICE_ERR_NULL;
if (inited) return DEVICE_ERR_INITED;
if ((thread_alert = osThreadGetId()) == NULL) return DEVICE_ERR_NULL;
BSP_UART_RegisterCallback(BSP_UART_DR16, BSP_UART_RX_CPLT_CB,
DR16_RxCpltCallback);
inited = true;
return DEVICE_OK;
}
int8_t DR16_Restart(void) {
__HAL_UART_DISABLE(BSP_UART_GetHandle(BSP_UART_DR16));
__HAL_UART_ENABLE(BSP_UART_GetHandle(BSP_UART_DR16));
return DEVICE_OK;
}
int8_t DR16_StartDmaRecv(DR16_t *dr16) {
if (HAL_UART_Receive_DMA(BSP_UART_GetHandle(BSP_UART_DR16),
(uint8_t *)&(dr16->data),
sizeof(dr16->data)) == HAL_OK)
return DEVICE_OK;
return DEVICE_ERR;
}
bool DR16_WaitDmaCplt(uint32_t timeout) {
return (osThreadFlagsWait(SIGNAL_DR16_RAW_REDY, osFlagsWaitAll, timeout) ==
SIGNAL_DR16_RAW_REDY);
}

47
User/device/dr16.h
View File

@ -1,47 +0,0 @@
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ----------------------------------------------------------------- */
#include <cmsis_os2.h>
#include "component/user_math.h"
#include "device/device.h"
/* Exported constants ------------------------------------------------------- */
/* Exported macro ----------------------------------------------------------- */
/* Exported types ----------------------------------------------------------- */
typedef struct __packed {
uint16_t ch_r_x : 11;
uint16_t ch_r_y : 11;
uint16_t ch_l_x : 11;
uint16_t ch_l_y : 11;
uint8_t sw_r : 2;
uint8_t sw_l : 2;
int16_t x;
int16_t y;
int16_t z;
uint8_t press_l;
uint8_t press_r;
uint16_t key;
uint16_t res;
} DR16_Data_t;
typedef struct {
DEVICE_Header_t header;
DR16_Data_t data;
} DR16_t;
/* Exported functions prototypes -------------------------------------------- */
int8_t DR16_Init(DR16_t *dr16);
int8_t DR16_Restart(void);
int8_t DR16_StartDmaRecv(DR16_t *dr16);
bool DR16_WaitDmaCplt(uint32_t timeout);
#ifdef __cplusplus
}
#endif

39
User/device/motor.c
View File

@ -1,39 +0,0 @@
/*
DR16接收机
*/
/* Includes ----------------------------------------------------------------- */
#include "motor.h"
#include <string.h>
/* Private define ----------------------------------------------------------- */
/* Private macro ------------------------------------------------------------ */
/* Private typedef ---------------------------------------------------------- */
/* Private variables -------------------------------------------------------- */
/* Private function -------------------------------------------------------- */
/* Exported functions ------------------------------------------------------- */
float MOTOR_GetRotorAbsAngle(const MOTOR_t *motor) {
if (motor == NULL) return DEVICE_ERR_NULL;
return motor->feedback.rotor_abs_angle;
}
float MOTOR_GetRotorSpeed(const MOTOR_t *motor) {
if (motor == NULL) return DEVICE_ERR_NULL;
return motor->feedback.rotor_speed;
}
float MOTOR_GetTorqueCurrent(const MOTOR_t *motor) {
if (motor == NULL) return DEVICE_ERR_NULL;
return motor->feedback.torque_current;
}
float MOTOR_GetTemp(const MOTOR_t *motor) {
if (motor == NULL) return DEVICE_ERR_NULL;
return motor->feedback.temp;
}

52
User/device/motor.h
View File

@ -1,52 +0,0 @@
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ----------------------------------------------------------------- */
#include "device/device.h"
/* Exported constants ------------------------------------------------------- */
/* Exported macro ----------------------------------------------------------- */
/* Exported types ----------------------------------------------------------- */
typedef struct {
float rotor_abs_angle; /* 转子绝对角度 */
float rotor_speed; /* 实际转子转速 */
float torque_current; /* 转矩电流 */
float temp; /* 温度 */
} MOTOR_Feedback_t;
/**
* @brief mit电机输出参数结构体
*/
typedef struct {
float torque; /* 目标力矩 */
float velocity; /* 目标速度 */
float angle; /* 目标位置 */
float kp; /* 位置环增益 */
float kd; /* 速度环增益 */
} MOTOR_MIT_Output_t;
/**
* @brief 转矩电流控制模式参数结构体
*/
typedef struct {
float current; /* 目标电流 */
} MOTOR_Current_Output_t;
typedef struct {
DEVICE_Header_t header;
bool reverse; /* 是否反装 true表示反装 */
MOTOR_Feedback_t feedback;
} MOTOR_t;
/* Exported functions prototypes -------------------------------------------- */
float MOTOR_GetRotorAbsAngle(const MOTOR_t *motor);
float MOTOR_GetRotorSpeed(const MOTOR_t *motor);
float MOTOR_GetTorqueCurrent(const MOTOR_t *motor);
float MOTOR_GetTemp(const MOTOR_t *motor);
#ifdef __cplusplus
}
#endif

316
User/device/motor_lk.c
View File

@ -1,316 +0,0 @@
/*
LK电机驱动
*/
/* Includes ----------------------------------------------------------------- */
#include "motor_lk.h"
#include <stdbool.h>
#include <string.h>
#include "bsp/can.h"
#include "bsp/mm.h"
#include "bsp/time.h"
#include "component/user_math.h"
/* Private define ----------------------------------------------------------- */
#define LK_CTRL_ID_BASE (0x140)
#define LK_FB_ID_BASE (0x240)
// LK电机命令字节定义
#define LK_CMD_FEEDBACK (0x9C) // 反馈命令字节
#define LK_CMD_MOTOR_OFF (0x80) // 电机关闭命令
#define LK_CMD_MOTOR_ON (0x88) // 电机运行命令
#define LK_CMD_TORQUE_CTRL (0xA1) // 转矩闭环控制命令
// LK电机参数定义
#define LK_CURR_LSB_MF (33.0f / 4096.0f) // MF电机转矩电流分辨率 A/LSB
#define LK_CURR_LSB_MG (66.0f / 4096.0f) // MG电机转矩电流分辨率 A/LSB
#define LK_POWER_RANGE_MS (1000) // MS电机功率范围 ±1000
#define LK_TORQUE_RANGE (2048) // 转矩控制值范围 ±2048
#define LK_TORQUE_CURRENT_MF (16.5f) // MF电机最大转矩电流 A
#define LK_TORQUE_CURRENT_MG (33.0f) // MG电机最大转矩电流 A
#define MOTOR_TX_BUF_SIZE (8)
#define MOTOR_RX_BUF_SIZE (8)
// 编码器分辨率定义
#define LK_ENC_14BIT_MAX (16383) // 14位编码器最大值
#define LK_ENC_15BIT_MAX (32767) // 15位编码器最大值
#define LK_ENC_16BIT_MAX (65535) // 16位编码器最大值
/* Private macro ------------------------------------------------------------ */
/* Private typedef ---------------------------------------------------------- */
/* Private variables -------------------------------------------------------- */
static MOTOR_LK_CANManager_t *can_managers[BSP_CAN_NUM] = {NULL};
/* Private functions -------------------------------------------------------- */
static float MOTOR_LK_GetCurrentLSB(MOTOR_LK_Module_t module) {
switch (module) {
case MOTOR_LK_MF9025:
case MOTOR_LK_MF9035:
return LK_CURR_LSB_MF;
default:
return LK_CURR_LSB_MG; // 默认使用MG的分辨率
}
}
static uint16_t MOTOR_LK_GetEncoderMax(MOTOR_LK_Module_t module) {
// 根据电机型号返回编码器最大值，这里假设都使用16位编码器
// 实际使用时需要根据具体电机型号配置
return LK_ENC_16BIT_MAX;
}
static MOTOR_LK_CANManager_t* MOTOR_LK_GetCANManager(BSP_CAN_t can) {
if (can >= BSP_CAN_NUM) return NULL;
return can_managers[can];
}
static int8_t MOTOR_LK_CreateCANManager(BSP_CAN_t can) {
if (can >= BSP_CAN_NUM) return DEVICE_ERR;
if (can_managers[can] != NULL) return DEVICE_OK;
can_managers[can] = (MOTOR_LK_CANManager_t*)BSP_Malloc(sizeof(MOTOR_LK_CANManager_t));
if (can_managers[can] == NULL) return DEVICE_ERR;
memset(can_managers[can], 0, sizeof(MOTOR_LK_CANManager_t));
can_managers[can]->can = can;
return DEVICE_OK;
}
static void MOTOR_LK_Decode(MOTOR_LK_t *motor, BSP_CAN_Message_t *msg) {
// 检查命令字节是否为反馈命令
if (msg->data[0] != LK_CMD_FEEDBACK) {
// 如果不是标准反馈命令，可能是其他格式的数据
// 临时跳过命令字节检查，直接解析数据
// return;
}
// 解析温度 (DATA[1])
motor->motor.feedback.temp = (int8_t)msg->data[1];
// 解析转矩电流值或功率值 (DATA[2], DATA[3])
int16_t raw_current_or_power = (int16_t)((msg->data[3] << 8) | msg->data[2]);
// 根据电机类型解析电流或功率
switch (motor->param.module) {
case MOTOR_LK_MF9025:
case MOTOR_LK_MF9035:
motor->motor.feedback.torque_current = raw_current_or_power * MOTOR_LK_GetCurrentLSB(motor->param.module);
break;
default:
motor->motor.feedback.torque_current = (float)raw_current_or_power;
break;
}
// 解析转速 (DATA[4], DATA[5]) - 单位：1dps/LSB
int16_t raw_speed = (int16_t)((msg->data[5] << 8) | msg->data[4]);
motor->motor.feedback.rotor_speed = motor->param.reverse ? -raw_speed : raw_speed;
// 解析编码器值 (DATA[6], DATA[7])
uint16_t raw_encoder = (uint16_t)((msg->data[7] << 8) | msg->data[6]);
uint16_t encoder_max = MOTOR_LK_GetEncoderMax(motor->param.module);
// 将编码器值转换为弧度 (0 ~ 2π)
float angle = (float)raw_encoder / (float)encoder_max * M_2PI;
motor->motor.feedback.rotor_abs_angle = motor->param.reverse ? (M_2PI - angle) : angle;
}
/* Exported functions ------------------------------------------------------- */
int8_t MOTOR_LK_Register(MOTOR_LK_Param_t *param) {
if (param == NULL) return DEVICE_ERR_NULL;
if (MOTOR_LK_CreateCANManager(param->can) != DEVICE_OK) return DEVICE_ERR;
MOTOR_LK_CANManager_t *manager = MOTOR_LK_GetCANManager(param->can);
if (manager == NULL) return DEVICE_ERR;
// 检查是否已注册
for (int i = 0; i < manager->motor_count; i++) {
if (manager->motors[i] && manager->motors[i]->param.id == param->id) {
return DEVICE_ERR_INITED;
}
}
// 检查数量
if (manager->motor_count >= MOTOR_LK_MAX_MOTORS) return DEVICE_ERR;
// 创建新电机实例
MOTOR_LK_t *new_motor = (MOTOR_LK_t*)BSP_Malloc(sizeof(MOTOR_LK_t));
if (new_motor == NULL) return DEVICE_ERR;
memcpy(&new_motor->param, param, sizeof(MOTOR_LK_Param_t));
memset(&new_motor->motor, 0, sizeof(MOTOR_t));
new_motor->motor.reverse = param->reverse;
// 对于某些LK电机，反馈数据可能通过命令ID发送
// 根据实际测试，使用命令ID接收反馈数据
uint16_t feedback_id = param->id; // 使用命令ID作为反馈ID
// 注册CAN接收ID
if (BSP_CAN_RegisterId(param->can, feedback_id, 3) != BSP_OK) {
BSP_Free(new_motor);
return DEVICE_ERR;
}
manager->motors[manager->motor_count] = new_motor;
manager->motor_count++;
return DEVICE_OK;
}
int8_t MOTOR_LK_Update(MOTOR_LK_Param_t *param) {
if (param == NULL) return DEVICE_ERR_NULL;
MOTOR_LK_CANManager_t *manager = MOTOR_LK_GetCANManager(param->can);
if (manager == NULL) return DEVICE_ERR_NO_DEV;
for (int i = 0; i < manager->motor_count; i++) {
MOTOR_LK_t *motor = manager->motors[i];
if (motor && motor->param.id == param->id) {
// 对于某些LK电机，反馈数据通过命令ID发送
uint16_t feedback_id = param->id;
BSP_CAN_Message_t rx_msg;
if (BSP_CAN_GetMessage(param->can, feedback_id, &rx_msg, BSP_CAN_TIMEOUT_IMMEDIATE) != BSP_OK) {
uint64_t now_time = BSP_TIME_Get();
if (now_time - motor->motor.header.last_online_time > 1000) {
motor->motor.header.online = false;
return DEVICE_ERR_NO_DEV;
}
return DEVICE_ERR;
}
motor->motor.header.online = true;
motor->motor.header.last_online_time = BSP_TIME_Get();
MOTOR_LK_Decode(motor, &rx_msg);
return DEVICE_OK;
}
}
return DEVICE_ERR_NO_DEV;
}
int8_t MOTOR_LK_UpdateAll(void) {
int8_t ret = DEVICE_OK;
for (int can = 0; can < BSP_CAN_NUM; can++) {
MOTOR_LK_CANManager_t *manager = MOTOR_LK_GetCANManager((BSP_CAN_t)can);
if (manager == NULL) continue;
for (int i = 0; i < manager->motor_count; i++) {
MOTOR_LK_t *motor = manager->motors[i];
if (motor != NULL) {
if (MOTOR_LK_Update(&motor->param) != DEVICE_OK) {
ret = DEVICE_ERR;
}
}
}
}
return ret;
}
int8_t MOTOR_LK_SetOutput(MOTOR_LK_Param_t *param, float value) {
if (param == NULL) return DEVICE_ERR_NULL;
MOTOR_LK_CANManager_t *manager = MOTOR_LK_GetCANManager(param->can);
if (manager == NULL) return DEVICE_ERR_NO_DEV;
// 限制输出值范围
if (value > 1.0f) value = 1.0f;
if (value < -1.0f) value = -1.0f;
MOTOR_LK_t *motor = MOTOR_LK_GetMotor(param);
if (motor == NULL) return DEVICE_ERR_NO_DEV;
// 根据反转参数调整输出
float output = param->reverse ? -value : value;
// 转矩闭环控制命令 - 将输出值转换为转矩控制值
int16_t torque_control = (int16_t)(output * (float)LK_TORQUE_RANGE);
// 构建CAN帧
BSP_CAN_StdDataFrame_t tx_frame;
tx_frame.id = param->id;
tx_frame.dlc = MOTOR_TX_BUF_SIZE;
tx_frame.data[0] = LK_CMD_TORQUE_CTRL;
tx_frame.data[1] = 0x00;
tx_frame.data[2] = 0x00;
tx_frame.data[3] = 0x00;
tx_frame.data[4] = (uint8_t)(torque_control & 0xFF);
tx_frame.data[5] = (uint8_t)((torque_control >> 8) & 0xFF);
tx_frame.data[6] = 0x00;
tx_frame.data[7] = 0x00;
return BSP_CAN_TransmitStdDataFrame(param->can, &tx_frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR;
}
int8_t MOTOR_LK_Ctrl(MOTOR_LK_Param_t *param) {
// 对于LK电机，每次设置输出时就直接发送控制命令
// 这个函数可以用于发送其他控制命令，如电机开启/关闭
return DEVICE_OK;
}
int8_t MOTOR_LK_MotorOn(MOTOR_LK_Param_t *param) {
if (param == NULL) return DEVICE_ERR_NULL;
BSP_CAN_StdDataFrame_t tx_frame;
tx_frame.id = param->id;
tx_frame.dlc = MOTOR_TX_BUF_SIZE;
// 电机运行命令
tx_frame.data[0] = LK_CMD_MOTOR_ON; // 命令字节
tx_frame.data[1] = 0x00;
tx_frame.data[2] = 0x00;
tx_frame.data[3] = 0x00;
tx_frame.data[4] = 0x00;
tx_frame.data[5] = 0x00;
tx_frame.data[6] = 0x00;
tx_frame.data[7] = 0x00;
return BSP_CAN_TransmitStdDataFrame(param->can, &tx_frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR;
}
int8_t MOTOR_LK_MotorOff(MOTOR_LK_Param_t *param) {
if (param == NULL) return DEVICE_ERR_NULL;
BSP_CAN_StdDataFrame_t tx_frame;
tx_frame.id = param->id;
tx_frame.dlc = MOTOR_TX_BUF_SIZE;
// 电机关闭命令
tx_frame.data[0] = LK_CMD_MOTOR_OFF; // 命令字节
tx_frame.data[1] = 0x00;
tx_frame.data[2] = 0x00;
tx_frame.data[3] = 0x00;
tx_frame.data[4] = 0x00;
tx_frame.data[5] = 0x00;
tx_frame.data[6] = 0x00;
tx_frame.data[7] = 0x00;
return BSP_CAN_TransmitStdDataFrame(param->can, &tx_frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR;
}
MOTOR_LK_t* MOTOR_LK_GetMotor(MOTOR_LK_Param_t *param) {
if (param == NULL) return NULL;
MOTOR_LK_CANManager_t *manager = MOTOR_LK_GetCANManager(param->can);
if (manager == NULL) return NULL;
for (int i = 0; i < manager->motor_count; i++) {
MOTOR_LK_t *motor = manager->motors[i];
if (motor && motor->param.id == param->id) {
return motor;
}
}
return NULL;
}
int8_t MOTOR_LK_Relax(MOTOR_LK_Param_t *param) {
return MOTOR_LK_SetOutput(param, 0.0f);
}
int8_t MOTOR_LK_Offine(MOTOR_LK_Param_t *param) {
MOTOR_LK_t *motor = MOTOR_LK_GetMotor(param);
if (motor) {
motor->motor.header.online = false;
return DEVICE_OK;
}
return DEVICE_ERR_NO_DEV;
}

119
User/device/motor_lk.h
View File

@ -1,119 +0,0 @@
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ----------------------------------------------------------------- */
#include "device/device.h"
#include "device/motor.h"
#include "bsp/can.h"
/* Exported constants ------------------------------------------------------- */
#define MOTOR_LK_MAX_MOTORS 32
/* Exported macro ----------------------------------------------------------- */
/* Exported types ----------------------------------------------------------- */
typedef enum {
MOTOR_LK_MF9025,
MOTOR_LK_MF9035,
} MOTOR_LK_Module_t;
/*每个电机需要的参数*/
typedef struct {
BSP_CAN_t can;
uint16_t id;
MOTOR_LK_Module_t module;
bool reverse;
} MOTOR_LK_Param_t;
/*电机实例*/
typedef struct{
MOTOR_LK_Param_t param;
MOTOR_t motor;
} MOTOR_LK_t;
/*CAN管理器，管理一个CAN总线上所有的电机*/
typedef struct {
BSP_CAN_t can;
MOTOR_LK_t *motors[MOTOR_LK_MAX_MOTORS];
uint8_t motor_count;
} MOTOR_LK_CANManager_t;
/* Exported functions prototypes -------------------------------------------- */
/**
* @brief 注册一个LK电机
* @param param 电机参数
* @return
*/
int8_t MOTOR_LK_Register(MOTOR_LK_Param_t *param);
/**
* @brief 更新指定电机数据
* @param param 电机参数
* @return
*/
int8_t MOTOR_LK_Update(MOTOR_LK_Param_t *param);
/**
* @brief 设置一个电机的输出
* @param param 电机参数
* @param value 输出值，范围[-1.0, 1.0]
* @return
*/
int8_t MOTOR_LK_SetOutput(MOTOR_LK_Param_t *param, float value);
/**
* @brief 发送控制命令到电机，注意一个CAN可以控制多个电机，所以只需要发送一次即可
* @param param 电机参数
* @return
*/
int8_t MOTOR_LK_Ctrl(MOTOR_LK_Param_t *param);
/**
* @brief 发送电机开启命令
* @param param 电机参数
* @return
*/
int8_t MOTOR_LK_MotorOn(MOTOR_LK_Param_t *param);
/**
* @brief 发送电机关闭命令
* @param param 电机参数
* @return
*/
int8_t MOTOR_LK_MotorOff(MOTOR_LK_Param_t *param);
/**
* @brief 获取指定电机的实例指针
* @param param 电机参数
* @return
*/
MOTOR_LK_t* MOTOR_LK_GetMotor(MOTOR_LK_Param_t *param);
/**
* @brief 使电机松弛（设置输出为0）
* @param param
* @return
*/
int8_t MOTOR_LK_Relax(MOTOR_LK_Param_t *param);
/**
* @brief 使电机离线（设置在线状态为false）
* @param param
* @return
*/
int8_t MOTOR_LK_Offine(MOTOR_LK_Param_t *param);
/**
* @brief
* @param
* @return
*/
int8_t MOTOR_LK_UpdateAll(void);
#ifdef __cplusplus
}
#endif

478
User/device/motor_lz.c
View File

@ -1,478 +0,0 @@
/*
灵足电机驱动
灵足电机通信协议：
- CAN 2.0通信接口，波特率1Mbps
- 采用扩展帧格式(29位ID)
- ID格式：Bit28~24(通信类型) + Bit23~8(数据区2) + Bit7~0(目标地址)
*/
/* Includes ----------------------------------------------------------------- */
#include "motor_lz.h"
#include <stdint.h>
#include <stdbool.h>
#include <string.h>
#include "bsp/can.h"
#include "bsp/mm.h"
#include "bsp/time.h"
#include "component/user_math.h"
/* Private define ----------------------------------------------------------- */
// 灵足电机协议参数
#define LZ_ANGLE_RANGE_RAD (12.57f) /* 角度范围 ±12.57 rad */
#define LZ_VELOCITY_RANGE_RAD_S (20.0f) /* 角速度范围 ±20 rad/s */
#define LZ_TORQUE_RANGE_NM (60.0f) /* 力矩范围 ±60 Nm */
#define LZ_KP_MAX (5000.0f) /* Kp最大值 */
#define LZ_KD_MAX (100.0f) /* Kd最大值 */
#define LZ_RAW_VALUE_MAX (65535) /* 16位原始值最大值 */
#define LZ_TEMP_SCALE (10.0f) /* 温度缩放因子 */
#define LZ_MAX_RECOVER_DIFF_RAD (0.28f)
#define MOTOR_TX_BUF_SIZE (8)
#define MOTOR_RX_BUF_SIZE (8)
/* Private macro ------------------------------------------------------------ */
MOTOR_LZ_MotionParam_t lz_recover_param = {
.target_angle = 0.0f,
.target_velocity = 0.0f,
.kp = 30.0f,
.kd = 1.0f,
.torque = 0.0f,
};
/* Private typedef ---------------------------------------------------------- */
/* Private variables -------------------------------------------------------- */
static MOTOR_LZ_CANManager_t *can_managers[BSP_CAN_NUM] = {NULL};
/* Private function prototypes ---------------------------------------------- */
static MOTOR_LZ_CANManager_t* MOTOR_LZ_GetCANManager(BSP_CAN_t can);
static int8_t MOTOR_LZ_CreateCANManager(BSP_CAN_t can);
static void MOTOR_LZ_Decode(MOTOR_LZ_t *motor, BSP_CAN_Message_t *msg);
static uint32_t MOTOR_LZ_BuildExtID(MOTOR_LZ_CmdType_t cmd_type, uint16_t data2, uint8_t target_id);
static uint16_t MOTOR_LZ_FloatToRaw(float value, float max_value);
static float MOTOR_LZ_RawToFloat(uint16_t raw_value, float max_value);
static int8_t MOTOR_LZ_SendExtFrame(BSP_CAN_t can, uint32_t ext_id, uint8_t *data, uint8_t dlc);
static uint32_t MOTOR_LZ_IdParser(uint32_t original_id, BSP_CAN_FrameType_t frame_type);
/* Private functions -------------------------------------------------------- */
/**
* @brief 获取CAN管理器
*/
static MOTOR_LZ_CANManager_t* MOTOR_LZ_GetCANManager(BSP_CAN_t can) {
if (can >= BSP_CAN_NUM) return NULL;
return can_managers[can];
}
/**
* @brief 创建CAN管理器
*/
static int8_t MOTOR_LZ_CreateCANManager(BSP_CAN_t can) {
if (can >= BSP_CAN_NUM) return DEVICE_ERR;
if (can_managers[can] != NULL) return DEVICE_OK;
can_managers[can] = (MOTOR_LZ_CANManager_t*)BSP_Malloc(sizeof(MOTOR_LZ_CANManager_t));
if (can_managers[can] == NULL) return DEVICE_ERR;
memset(can_managers[can], 0, sizeof(MOTOR_LZ_CANManager_t));
can_managers[can]->can = can;
return DEVICE_OK;
}
/**
* @brief 构建扩展ID
*/
static uint32_t MOTOR_LZ_BuildExtID(MOTOR_LZ_CmdType_t cmd_type, uint16_t data2, uint8_t target_id) {
uint32_t ext_id = 0;
ext_id |= ((uint32_t)cmd_type & 0x1F) << 24; // Bit28~24: 通信类型
ext_id |= ((uint32_t)data2 & 0xFFFF) << 8; // Bit23~8: 数据区2
ext_id |= ((uint32_t)target_id & 0xFF); // Bit7~0: 目标地址
return ext_id;
}
/**
* @brief 浮点值转换为原始值（对称范围：-max_value ~ +max_value）
*/
static uint16_t MOTOR_LZ_FloatToRaw(float value, float max_value) {
// 限制范围
if (value > max_value) value = max_value;
if (value < -max_value) value = -max_value;
// 转换为0~65535范围，对应-max_value~max_value
return (uint16_t)((value + max_value) / (2.0f * max_value) * (float)LZ_RAW_VALUE_MAX);
}
/**
* @brief 浮点值转换为原始值（单向范围：0 ~ +max_value）
*/
static uint16_t MOTOR_LZ_FloatToRawPositive(float value, float max_value) {
// 限制范围
if (value > max_value) value = max_value;
if (value < 0.0f) value = 0.0f;
// 转换为0~65535范围，对应0~max_value
return (uint16_t)(value / max_value * (float)LZ_RAW_VALUE_MAX);
}
/**
* @brief 原始值转换为浮点值
*/
static float MOTOR_LZ_RawToFloat(uint16_t raw_value, float max_value) {
// 将0~65535范围转换为-max_value~max_value
return ((float)raw_value / (float)LZ_RAW_VALUE_MAX) * (2.0f * max_value) - max_value;
}
/**
* @brief 发送扩展帧
*/
static int8_t MOTOR_LZ_SendExtFrame(BSP_CAN_t can, uint32_t ext_id, uint8_t *data, uint8_t dlc) {
BSP_CAN_ExtDataFrame_t tx_frame;
tx_frame.id = ext_id;
tx_frame.dlc = dlc;
if (data != NULL) {
memcpy(tx_frame.data, data, dlc);
} else {
memset(tx_frame.data, 0, dlc);
}
return BSP_CAN_TransmitExtDataFrame(can, &tx_frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR;
}
/**
* @brief 灵足电机ID解析器
* @param original_id 原始CAN ID（29位扩展帧）
* @param frame_type 帧类型
* @return 解析后的ID（用于队列匹配）
*
* 灵足电机扩展ID格式：
* Bit28~24: 通信类型 (0x1=运控控制, 0x2=反馈数据, 0x3=使能, 0x4=停止, 0x6=设零位)
* Bit23~8: 数据区2 (根据通信类型而定)
* Bit7~0: 目标地址 (目标电机CAN ID)
*/
static uint32_t MOTOR_LZ_IdParser(uint32_t original_id, BSP_CAN_FrameType_t frame_type) {
// 只处理扩展数据帧
if (frame_type != BSP_CAN_FRAME_EXT_DATA) {
return original_id; // 非扩展帧直接返回原始ID
}
// 解析扩展ID各个字段
uint8_t cmd_type = (original_id >> 24) & 0x1F; // Bit28~24: 通信类型
uint16_t data2 = (original_id >> 8) & 0xFFFF; // Bit23~8: 数据区2
uint8_t host_id = (uint8_t)(original_id & 0xFF); // Bit7~0: 主机CAN ID
// 对于反馈数据帧，我们使用特殊的解析规则
if (cmd_type == MOTOR_LZ_CMD_FEEDBACK) {
// 反馈数据的data2字段包含：
// Bit8~15: 当前电机CAN ID
// Bit16~21: 故障信息
// Bit22~23: 模式状态
uint8_t motor_can_id = data2 & 0xFF; // bit8~15: 当前电机CAN ID
// 返回格式化的ID，便于匹配
// 格式：0x02HHMMTT (02=反馈命令, HH=主机ID, MM=电机ID, TT=主机ID)
return (0x02000000) | (host_id << 16) | (motor_can_id << 8) | host_id;
}
// 对于其他命令类型，直接返回原始ID
return original_id;
}
/**
* @brief 解码灵足电机反馈数据
*/
static void MOTOR_LZ_Decode(MOTOR_LZ_t *motor, BSP_CAN_Message_t *msg) {
if (motor == NULL || msg == NULL) return;
uint8_t cmd_type = (msg->original_id >> 24) & 0x1F;
if (cmd_type != MOTOR_LZ_CMD_FEEDBACK) return;
uint16_t id_data2 = (msg->original_id >> 8) & 0xFFFF;
uint8_t motor_can_id = id_data2 & 0xFF;
uint8_t fault_info = (id_data2 >> 8) & 0x3F;
uint8_t mode_state = (id_data2 >> 14) & 0x03;
motor->lz_feedback.motor_can_id = motor_can_id;
motor->lz_feedback.fault.under_voltage = (fault_info & 0x01) != 0;
motor->lz_feedback.fault.driver_fault = (fault_info & 0x02) != 0;
motor->lz_feedback.fault.over_temp = (fault_info & 0x04) != 0;
motor->lz_feedback.fault.encoder_fault = (fault_info & 0x08) != 0;
motor->lz_feedback.fault.stall_overload = (fault_info & 0x10) != 0;
motor->lz_feedback.fault.uncalibrated = (fault_info & 0x20) != 0;
motor->lz_feedback.state = (MOTOR_LZ_State_t)mode_state;
// 反馈解码并自动反向
uint16_t raw_angle = (uint16_t)((msg->data[0] << 8) | msg->data[1]);
float angle = MOTOR_LZ_RawToFloat(raw_angle, LZ_ANGLE_RANGE_RAD);
uint16_t raw_velocity = (uint16_t)((msg->data[2] << 8) | msg->data[3]);
float velocity = MOTOR_LZ_RawToFloat(raw_velocity, LZ_VELOCITY_RANGE_RAD_S);
uint16_t raw_torque = (uint16_t)((msg->data[4] << 8) | msg->data[5]);
float torque = MOTOR_LZ_RawToFloat(raw_torque, LZ_TORQUE_RANGE_NM);
while (angle <0){
angle += M_2PI;
}
while (angle > M_2PI){
angle -= M_2PI;
}
// 自动反向
if (motor->param.reverse) {
angle = M_2PI - angle;
velocity = -velocity;
torque = -torque;
}
motor->lz_feedback.current_angle = angle;
motor->lz_feedback.current_velocity = velocity;
motor->lz_feedback.current_torque = torque;
uint16_t raw_temp = (uint16_t)((msg->data[6] << 8) | msg->data[7]);
motor->lz_feedback.temperature = (float)raw_temp / LZ_TEMP_SCALE;
motor->motor.feedback.rotor_abs_angle = angle;
motor->motor.feedback.rotor_speed = velocity;
motor->motor.feedback.torque_current = torque;
motor->motor.feedback.temp = (int8_t)motor->lz_feedback.temperature;
motor->motor.header.online = true;
motor->motor.header.last_online_time = BSP_TIME_Get();
}
/* Exported functions ------------------------------------------------------- */
/**
* @brief 初始化灵足电机驱动系统
* @return 设备状态码
*/
int8_t MOTOR_LZ_Init(void) {
// 注册灵足电机专用的ID解析器
return BSP_CAN_RegisterIdParser(MOTOR_LZ_IdParser) == BSP_OK ? DEVICE_OK : DEVICE_ERR;
}
/**
* @brief 反初始化灵足电机驱动系统
* @return 设备状态码
*/
int8_t MOTOR_LZ_DeInit(void) {
// 注销ID解析器
return BSP_CAN_UnregisterIdParser() == BSP_OK ? DEVICE_OK : DEVICE_ERR;
}
int8_t MOTOR_LZ_Register(MOTOR_LZ_Param_t *param) {
if (param == NULL) return DEVICE_ERR_NULL;
if (MOTOR_LZ_CreateCANManager(param->can) != DEVICE_OK) return DEVICE_ERR;
MOTOR_LZ_CANManager_t *manager = MOTOR_LZ_GetCANManager(param->can);
if (manager == NULL) return DEVICE_ERR;
// 检查是否已注册
for (int i = 0; i < manager->motor_count; i++) {
if (manager->motors[i] && manager->motors[i]->param.motor_id == param->motor_id) {
return DEVICE_ERR; // 已注册
}
}
// 检查数量
if (manager->motor_count >= MOTOR_LZ_MAX_MOTORS) return DEVICE_ERR;
// 创建新电机实例
MOTOR_LZ_t *new_motor = (MOTOR_LZ_t*)BSP_Malloc(sizeof(MOTOR_LZ_t));
if (new_motor == NULL) return DEVICE_ERR;
memcpy(&new_motor->param, param, sizeof(MOTOR_LZ_Param_t));
memset(&new_motor->motor, 0, sizeof(MOTOR_t));
memset(&new_motor->lz_feedback, 0, sizeof(MOTOR_LZ_Feedback_t));
memset(&new_motor->motion_param, 0, sizeof(MOTOR_LZ_MotionParam_t));
new_motor->motor.reverse = param->reverse;
// 注册CAN接收ID - 使用解析后的反馈数据ID
// 构建反馈数据的原始扩展ID
// 反馈数据：data2包含电机ID(bit8~15)，target_id是主机ID
uint32_t original_feedback_id = MOTOR_LZ_BuildExtID(MOTOR_LZ_CMD_FEEDBACK, param->motor_id, param->host_id);
// 通过ID解析器得到解析后的ID
uint32_t parsed_feedback_id = MOTOR_LZ_IdParser(original_feedback_id, BSP_CAN_FRAME_EXT_DATA);
if (BSP_CAN_RegisterId(param->can, parsed_feedback_id, 3) != BSP_OK) {
BSP_Free(new_motor);
return DEVICE_ERR;
}
manager->motors[manager->motor_count] = new_motor;
manager->motor_count++;
return DEVICE_OK;
}
int8_t MOTOR_LZ_Update(MOTOR_LZ_Param_t *param) {
if (param == NULL) return DEVICE_ERR_NULL;
MOTOR_LZ_CANManager_t *manager = MOTOR_LZ_GetCANManager(param->can);
if (manager == NULL) return DEVICE_ERR_NO_DEV;
for (int i = 0; i < manager->motor_count; i++) {
MOTOR_LZ_t *motor = manager->motors[i];
if (motor && motor->param.motor_id == param->motor_id) {
// 获取反馈数据 - 使用解析后的ID
uint32_t original_feedback_id = MOTOR_LZ_BuildExtID(MOTOR_LZ_CMD_FEEDBACK, param->motor_id, param->host_id);
uint32_t parsed_feedback_id = MOTOR_LZ_IdParser(original_feedback_id, BSP_CAN_FRAME_EXT_DATA);
BSP_CAN_Message_t msg;
while (BSP_CAN_GetMessage(param->can, parsed_feedback_id, &msg, 0) == BSP_OK) {
MOTOR_LZ_Decode(motor, &msg);
}
return DEVICE_OK;
}
}
return DEVICE_ERR_NO_DEV;
}
int8_t MOTOR_LZ_UpdateAll(void) {
int8_t ret = DEVICE_OK;
for (int can = 0; can < BSP_CAN_NUM; can++) {
MOTOR_LZ_CANManager_t *manager = MOTOR_LZ_GetCANManager((BSP_CAN_t)can);
if (manager == NULL) continue;
for (int i = 0; i < manager->motor_count; i++) {
MOTOR_LZ_t *motor = manager->motors[i];
if (motor) {
if (MOTOR_LZ_Update(&motor->param) != DEVICE_OK) {
ret = DEVICE_ERR;
}
}
}
}
return ret;
}
int8_t MOTOR_LZ_MotionControl(MOTOR_LZ_Param_t *param, MOTOR_LZ_MotionParam_t *motion_param) {
if (param == NULL || motion_param == NULL) return DEVICE_ERR_NULL;
MOTOR_LZ_t *motor = MOTOR_LZ_GetMotor(param);
if (motor == NULL) return DEVICE_ERR_NO_DEV;
// 自动反向控制
MOTOR_LZ_MotionParam_t send_param = *motion_param;
if (param->reverse) {
send_param.target_angle = -send_param.target_angle;
send_param.target_velocity = -send_param.target_velocity;
send_param.torque = -send_param.torque;
}
memcpy(&motor->motion_param, motion_param, sizeof(MOTOR_LZ_MotionParam_t));
uint16_t raw_torque = MOTOR_LZ_FloatToRaw(send_param.torque, LZ_TORQUE_RANGE_NM);
uint32_t ext_id = MOTOR_LZ_BuildExtID(MOTOR_LZ_CMD_MOTION, raw_torque, param->motor_id);
uint8_t data[8];
uint16_t raw_angle = MOTOR_LZ_FloatToRaw(send_param.target_angle, LZ_ANGLE_RANGE_RAD);
data[0] = (raw_angle >> 8) & 0xFF;
data[1] = raw_angle & 0xFF;
uint16_t raw_velocity = MOTOR_LZ_FloatToRaw(send_param.target_velocity, LZ_VELOCITY_RANGE_RAD_S);
data[2] = (raw_velocity >> 8) & 0xFF;
data[3] = raw_velocity & 0xFF;
uint16_t raw_kp = MOTOR_LZ_FloatToRawPositive(send_param.kp, LZ_KP_MAX);
data[4] = (raw_kp >> 8) & 0xFF;
data[5] = raw_kp & 0xFF;
uint16_t raw_kd = MOTOR_LZ_FloatToRawPositive(send_param.kd, LZ_KD_MAX);
data[6] = (raw_kd >> 8) & 0xFF;
data[7] = raw_kd & 0xFF;
return MOTOR_LZ_SendExtFrame(param->can, ext_id, data, 8);
}
int8_t MOTOR_LZ_Enable(MOTOR_LZ_Param_t *param) {
if (param == NULL) return DEVICE_ERR_NULL;
// 构建扩展ID - 使能命令
uint32_t ext_id = MOTOR_LZ_BuildExtID(MOTOR_LZ_CMD_ENABLE, param->host_id, param->motor_id);
// 数据区清零
uint8_t data[8] = {0};
return MOTOR_LZ_SendExtFrame(param->can, ext_id, data, 8);
}
int8_t MOTOR_LZ_Disable(MOTOR_LZ_Param_t *param, bool clear_fault) {
if (param == NULL) return DEVICE_ERR_NULL;
// 构建扩展ID - 停止命令
uint32_t ext_id = MOTOR_LZ_BuildExtID(MOTOR_LZ_CMD_DISABLE, param->host_id, param->motor_id);
// 数据区
uint8_t data[8] = {0};
if (clear_fault) {
data[0] = 1; // Byte[0]=1时清故障
}
return MOTOR_LZ_SendExtFrame(param->can, ext_id, data, 8);
}
int8_t MOTOR_LZ_SetZero(MOTOR_LZ_Param_t *param) {
if (param == NULL) return DEVICE_ERR_NULL;
// 构建扩展ID - 设置零位命令
uint32_t ext_id = MOTOR_LZ_BuildExtID(MOTOR_LZ_CMD_SET_ZERO, param->host_id, param->motor_id);
// 数据区 - Byte[0]=1
uint8_t data[8] = {1, 0, 0, 0, 0, 0, 0, 0};
return MOTOR_LZ_SendExtFrame(param->can, ext_id, data, 8);
}
MOTOR_LZ_t* MOTOR_LZ_GetMotor(MOTOR_LZ_Param_t *param) {
if (param == NULL) return NULL;
MOTOR_LZ_CANManager_t *manager = MOTOR_LZ_GetCANManager(param->can);
if (manager == NULL) return NULL;
for (int i = 0; i < manager->motor_count; i++) {
MOTOR_LZ_t *motor = manager->motors[i];
if (motor && motor->param.motor_id == param->motor_id) {
return motor;
}
}
return NULL;
}
int8_t MOTOR_LZ_Relax(MOTOR_LZ_Param_t *param) {
return MOTOR_LZ_Disable(param, false);
}
int8_t MOTOR_LZ_Offline(MOTOR_LZ_Param_t *param) {
MOTOR_LZ_t *motor = MOTOR_LZ_GetMotor(param);
if (motor) {
motor->motor.header.online = false;
return DEVICE_OK;
}
return DEVICE_ERR_NO_DEV;
}
static MOTOR_LZ_Feedback_t* MOTOR_LZ_GetFeedback(MOTOR_LZ_Param_t *param) {
MOTOR_LZ_t *motor = MOTOR_LZ_GetMotor(param);
if (motor && motor->motor.header.online) {
return &motor->lz_feedback;
}
return NULL;
}
int8_t MOTOR_LZ_RecoverToZero(MOTOR_LZ_Param_t *param) {
if (param == NULL) return DEVICE_ERR_NULL;
MOTOR_LZ_t *motor = MOTOR_LZ_GetMotor(param);
if (motor == NULL) return DEVICE_ERR_NO_DEV;
// 获取当前角度
MOTOR_LZ_Feedback_t *feedback = MOTOR_LZ_GetFeedback(param);
if (feedback == NULL) return DEVICE_ERR_NO_DEV;
float current_angle = feedback->current_angle;
// 计算目标角度为0时的最短路径
float angle_diff = -current_angle; // 目标是0，所以差值就是-current_angle
// 限制最大差值，防止过大跳变
if (angle_diff > LZ_MAX_RECOVER_DIFF_RAD) angle_diff = LZ_MAX_RECOVER_DIFF_RAD;
if (angle_diff < -LZ_MAX_RECOVER_DIFF_RAD) angle_diff = -LZ_MAX_RECOVER_DIFF_RAD;
float target_angle = current_angle + angle_diff;
// 创建运控参数，设置位置和速度限制
MOTOR_LZ_MotionParam_t motion_param = lz_recover_param; // 使用预设的恢复参数
motion_param.target_angle = target_angle;
return MOTOR_LZ_MotionControl(param, &motion_param);
}

195
User/device/motor_lz.h
View File

@ -1,195 +0,0 @@
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ----------------------------------------------------------------- */
#include "device/device.h"
#include "device/motor.h"
#include "bsp/can.h"
/* Exported constants ------------------------------------------------------- */
#define MOTOR_LZ_MAX_MOTORS 32
/* Exported macro ----------------------------------------------------------- */
/* Exported types ----------------------------------------------------------- */
typedef enum {
MOTOR_LZ_RSO0,
MOTOR_LZ_RSO1,
MOTOR_LZ_RSO2,
MOTOR_LZ_RSO3,
MOTOR_LZ_RSO4,
MOTOR_LZ_RSO5,
MOTOR_LZ_RSO6,
} MOTOR_LZ_Module_t;
/* 灵足电机控制模式 */
typedef enum {
MOTOR_LZ_MODE_MOTION = 0x1, /* 运控模式 */
MOTOR_LZ_MODE_CURRENT = 0x2, /* 电流模式 */
MOTOR_LZ_MODE_VELOCITY = 0x3, /* 速度模式 */
MOTOR_LZ_MODE_POSITION = 0x4, /* 位置模式 */
} MOTOR_LZ_ControlMode_t;
/* 灵足电机通信类型 */
typedef enum {
MOTOR_LZ_CMD_MOTION = 0x1, /* 运控模式控制 */
MOTOR_LZ_CMD_FEEDBACK = 0x2, /* 电机反馈数据 */
MOTOR_LZ_CMD_ENABLE = 0x3, /* 电机使能运行 */
MOTOR_LZ_CMD_DISABLE = 0x4, /* 电机停止运行 */
MOTOR_LZ_CMD_SET_ZERO = 0x6, /* 设置电机机械零位 */
} MOTOR_LZ_CmdType_t;
/* 灵足电机运行状态 */
typedef enum {
MOTOR_LZ_STATE_RESET = 0, /* Reset模式[复位] */
MOTOR_LZ_STATE_CALI = 1, /* Cali模式[标定] */
MOTOR_LZ_STATE_MOTOR = 2, /* Motor模式[运行] */
} MOTOR_LZ_State_t;
/* 灵足电机故障信息 */
typedef struct {
bool uncalibrated; /* bit21: 未标定 */
bool stall_overload; /* bit20: 堵转过载故障 */
bool encoder_fault; /* bit19: 磁编码故障 */
bool over_temp; /* bit18: 过温 */
bool driver_fault; /* bit17: 驱动故障 */
bool under_voltage; /* bit16: 欠压故障 */
} MOTOR_LZ_Fault_t;
/* 灵足电机运控参数 */
typedef struct {
float target_angle; /* 目标角度 (-12.57f~12.57f rad) */
float target_velocity; /* 目标角速度 (-20~20 rad/s) */
float kp; /* 位置增益 (0.0~5000.0) */
float kd; /* 微分增益 (0.0~100.0) */
float torque; /* 力矩 (-60~60 Nm) */
} MOTOR_LZ_MotionParam_t;
/*每个电机需要的参数*/
typedef struct {
BSP_CAN_t can; /* CAN总线 */
uint8_t motor_id; /* 电机CAN ID */
uint8_t host_id; /* 主机CAN ID */
MOTOR_LZ_Module_t module; /* 电机型号 */
bool reverse; /* 是否反向 */
MOTOR_LZ_ControlMode_t mode; /* 控制模式 */
} MOTOR_LZ_Param_t;
/*电机反馈信息扩展*/
typedef struct {
float current_angle; /* 当前角度 (-12.57f~12.57f rad) */
float current_velocity; /* 当前角速度 (-20~20 rad/s) */
float current_torque; /* 当前力矩 (-60~60 Nm) */
float temperature; /* 当前温度 (摄氏度) */
MOTOR_LZ_State_t state; /* 运行状态 */
MOTOR_LZ_Fault_t fault; /* 故障信息 */
uint8_t motor_can_id; /* 当前电机CAN ID */
} MOTOR_LZ_Feedback_t;
/*电机实例*/
typedef struct {
MOTOR_LZ_Param_t param;
MOTOR_t motor;
MOTOR_LZ_Feedback_t lz_feedback; /* 灵足电机特有反馈信息 */
MOTOR_LZ_MotionParam_t motion_param; /* 运控模式参数 */
} MOTOR_LZ_t;
/*CAN管理器，管理一个CAN总线上所有的电机*/
typedef struct {
BSP_CAN_t can;
MOTOR_LZ_t *motors[MOTOR_LZ_MAX_MOTORS];
uint8_t motor_count;
} MOTOR_LZ_CANManager_t;
/* Exported functions prototypes -------------------------------------------- */
/**
* @brief 初始化灵足电机驱动系统
* @return 设备状态码
*/
int8_t MOTOR_LZ_Init(void);
/**
* @brief 反初始化灵足电机驱动系统
* @return 设备状态码
*/
int8_t MOTOR_LZ_DeInit(void);
/**
* @brief 注册一个灵足电机
* @param param 电机参数
* @return 设备状态码
*/
int8_t MOTOR_LZ_Register(MOTOR_LZ_Param_t *param);
/**
* @brief 更新指定电机数据
* @param param 电机参数
* @return 设备状态码
*/
int8_t MOTOR_LZ_Update(MOTOR_LZ_Param_t *param);
/**
* @brief 更新所有电机数据
* @return 设备状态码
*/
int8_t MOTOR_LZ_UpdateAll(void);
/**
* @brief 运控模式控制电机
* @param param 电机参数
* @param motion_param 运控参数
* @return 设备状态码
*/
int8_t MOTOR_LZ_MotionControl(MOTOR_LZ_Param_t *param, MOTOR_LZ_MotionParam_t *motion_param);
/**
* @brief 电机使能运行
* @param param 电机参数
* @return 设备状态码
*/
int8_t MOTOR_LZ_Enable(MOTOR_LZ_Param_t *param);
/**
* @brief 电机停止运行
* @param param 电机参数
* @param clear_fault 是否清除故障
* @return 设备状态码
*/
int8_t MOTOR_LZ_Disable(MOTOR_LZ_Param_t *param, bool clear_fault);
/**
* @brief 设置电机机械零位
* @param param 电机参数
* @return 设备状态码
*/
int8_t MOTOR_LZ_SetZero(MOTOR_LZ_Param_t *param);
/**
* @brief 获取指定电机的实例指针
* @param param 电机参数
* @return 电机实例指针，失败返回NULL
*/
MOTOR_LZ_t* MOTOR_LZ_GetMotor(MOTOR_LZ_Param_t *param);
/**
* @brief 使电机松弛（发送停止命令）
* @param param 电机参数
* @return 设备状态码
*/
int8_t MOTOR_LZ_Relax(MOTOR_LZ_Param_t *param);
/**
* @brief 使电机离线（设置在线状态为false）
* @param param 电机参数
* @return 设备状态码
*/
int8_t MOTOR_LZ_Offline(MOTOR_LZ_Param_t *param);
int8_t MOTOR_LZ_RecoverToZero(MOTOR_LZ_Param_t *param);
#ifdef __cplusplus
}
#endif

339
User/module/balance_chassis.c
View File

@ -1,339 +0,0 @@
#include "module/balance_chassis.h"
#include "bsp/time.h"
#include "bsp/can.h"
#include "component/user_math.h"
#include "component/kinematics.h"
#include <math.h>
#include <string.h>
float fn=0.0f;
float tp=0.0f;
float t1=0.0f;
float t2=0.0f;
static int8_t Chassis_SetMode(Chassis_t *c, Chassis_Mode_t mode) {
if (c == NULL) return CHASSIS_ERR_NULL; /* 主结构体不能为空 */
if (mode == c->mode) return CHASSIS_OK; /* 模式未改变直接返回 */
MOTOR_LK_MotorOn(&c->param->wheel_motors[0]);
MOTOR_LK_MotorOn(&c->param->wheel_motors[1]);
for (int i = 0; i < 4; i++) {
MOTOR_LZ_Enable(&c->param->joint_motors[i]);
}
c->mode = mode;
c->state = 0; // 重置状态，确保每次切换模式时都重新初始化
return CHASSIS_OK;
}
/* 更新机体状态估计 */
static void Chassis_UpdateChassisState(Chassis_t *c) {
if (c == NULL) return;
// 从轮子编码器估计机体速度 (参考C++代码)
float left_wheel_speed_dps = c->feedback.wheel[0].rotor_speed; // dps (度每秒)
float right_wheel_speed_dps = c->feedback.wheel[1].rotor_speed; // dps (度每秒)
// 将dps转换为rad/s
float left_wheel_speed = left_wheel_speed_dps * M_PI / 180.0f; // rad/s
float right_wheel_speed = right_wheel_speed_dps * M_PI / 180.0f; // rad/s
float wheel_radius = 0.072f;
float left_wheel_linear_vel = left_wheel_speed * wheel_radius;
float right_wheel_linear_vel = right_wheel_speed * wheel_radius;
// 机体x方向速度 (轮子中心速度)
c->chassis_state.last_velocity_x = c->chassis_state.velocity_x;
c->chassis_state.velocity_x = (left_wheel_linear_vel + right_wheel_linear_vel) / 2.0f;
// 积分得到位置
c->chassis_state.position_x += c->chassis_state.velocity_x * c->dt;
}
/* 执行LQR控制 */
static int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
if (c == NULL || c_cmd == NULL) return -1;
// 构建当前状态
LQR_State_t current_state;
float left_leg_length, left_leg_angle, left_leg_d_length, left_leg_d_angle;
float right_leg_length, right_leg_angle, right_leg_d_length, right_leg_d_angle;
// 获取等效摆动杆状态
VMC_GetVirtualLegState(&c->vmc_[0], &left_leg_length, &left_leg_angle, &left_leg_d_length, &left_leg_d_angle);
VMC_GetVirtualLegState(&c->vmc_[1], &right_leg_length, &right_leg_angle, &right_leg_d_length, &right_leg_d_angle);
LQR_BuildStateFromSensors(
c->chassis_state.position_x,
c->chassis_state.velocity_x,
c->feedback.imu.euler.yaw,
c->feedback.imu.gyro.z,
left_leg_angle,
left_leg_d_angle,
right_leg_angle,
right_leg_d_angle,
c->feedback.imu.euler.pit,
c->feedback.imu.gyro.y,
&current_state
);
// 设置参考状态
LQR_State_t reference_state = {0};
reference_state.s = 0.0f; // 期望位移设为0（相对平衡位置）
reference_state.ds = c_cmd->move_vec.vx; // 期望速度
reference_state.phi = 0.0f; // 期望yaw角度
reference_state.dphi = c_cmd->move_vec.wz; // 期望yaw角速度
// 其他状态保持为0（平衡状态）
// 更新LQR控制器状态
LQR_UpdateState(&c->lqr, &current_state);
LQR_SetReference(&c->lqr, &reference_state);
// 计算控制输出
if (LQR_ComputeControl(&c->lqr) != 0) {
return -1;
}
// 获取控制输出
LQR_Control_t lqr_output;
LQR_GetControl(&c->lqr, &lqr_output);
// 分配力矩到电机
// 轮毂电机 (考虑减速比)
// float wheel_gear_ratio = 19.2f;
// MOTOR_LK_SetTorque(&c->param->wheel_motors[0], lqr_output.T_wl / wheel_gear_ratio);
// MOTOR_LK_SetTorque(&c->param->wheel_motors[1], lqr_output.T_wr / wheel_gear_ratio);
c->output.wheel[0] = lqr_output.T_wl/2.5; // 轮子电机输出
c->output.wheel[1] = lqr_output.T_wr/2.5;
// 通过VMC将虚拟力转换为关节力矩
// 左腿
float F_virtual_left = lqr_output.T_bl; // 简化映射，实际需要更复杂的转换
// float T_virtual_left = 0.0f;
float T_virtual_left = lqr_output.T_wl; // 左腿虚拟摆动力矩
VMC_InverseSolve(&c->vmc_[0], F_virtual_left, T_virtual_left);
float tau_left_front, tau_left_rear;
VMC_GetJointTorques(&c->vmc_[0], &tau_left_front, &tau_left_rear);
// 右腿
float F_virtual_right = lqr_output.T_br;
// float T_virtual_right = 0.0f;
float T_virtual_right = lqr_output.T_wr; // 右腿虚拟摆动力矩
VMC_InverseSolve(&c->vmc_[1], F_virtual_right, T_virtual_right);
float tau_right_front, tau_right_rear;
VMC_GetJointTorques(&c->vmc_[1], &tau_right_front, &tau_right_rear);
// 输出到关节电机
// MOTOR_LZ_SetTorque(&c->param->joint_motors[0], tau_left_rear); // 左后
// MOTOR_LZ_SetTorque(&c->param->joint_motors[1], tau_left_front); // 左前
// MOTOR_LZ_SetTorque(&c->param->joint_motors[2], tau_right_front);// 右前
// MOTOR_LZ_SetTorque(&c->param->joint_motors[3], tau_right_rear); // 右后
c->output.joint[0].torque = tau_left_rear;
c->output.joint[1].torque = tau_left_front;
c->output.joint[2].torque = tau_right_front;
c->output.joint[3].torque = tau_right_rear;
return 0;
}
int8_t Chassis_Init(Chassis_t *c, Chassis_Params_t *param, float target_freq){
if (c == NULL || param == NULL || target_freq <= 0.0f) {
return -1; // 参数错误
}
c->param = param;
/*初始化can*/
BSP_CAN_Init();
/*初始化和注册所有电机*/
MOTOR_LZ_Init();
for (int i = 0; i < 4; i++) {
MOTOR_LZ_Register(&c->param->joint_motors[i]);
}
for (int i = 0; i < 2; i++) {
MOTOR_LK_Register(&c->param->wheel_motors[i]);
}
/*初始化VMC控制器*/
VMC_Init(&c->vmc_[0], &param->vmc_param[0], target_freq);
VMC_Init(&c->vmc_[1], &param->vmc_param[1], target_freq);
/*初始化LQR控制器*/
LQR_Init(&c->lqr, param->lqr_param.max_wheel_torque, param->lqr_param.max_joint_torque);
LQR_SetGainMatrix(&c->lqr, &param->lqr_gains);
/*初始化机体状态*/
c->chassis_state.position_x = 0.0f;
c->chassis_state.velocity_x = 0.0f;
c->chassis_state.last_velocity_x = 0.0f;
return CHASSIS_OK;
}
int8_t Chassis_UpdateFeedback(Chassis_t *c){
if (c == NULL) {
return -1; // 参数错误
}
/*更新电机反馈*/
for (int i = 0; i < 4; i++) {
MOTOR_LZ_Update(&c->param->joint_motors[i]);
}
MOTOR_LK_Update(&c->param->wheel_motors[0]);
MOTOR_LK_Update(&c->param->wheel_motors[1]);
/*将电机反馈数据赋值到标准反馈结构体，并检查是否离线*/
// 更新关节电机反馈并检查离线状态
for (int i = 0; i < 4; i++) {
MOTOR_LZ_t *joint_motor = MOTOR_LZ_GetMotor(&c->param->joint_motors[i]);
if (joint_motor != NULL) {
c->feedback.joint[i] = joint_motor->motor.feedback;
c->feedback.joint[i].rotor_abs_angle = joint_motor->motor.feedback.rotor_abs_angle - M_PI; // 机械零点调整
}
}
// 更新轮子电机反馈并检查离线状态
for (int i = 0; i < 2; i++) {
MOTOR_LK_t *wheel_motor = MOTOR_LK_GetMotor(&c->param->wheel_motors[i]);
if (wheel_motor != NULL) {
c->feedback.wheel[i] = wheel_motor->motor.feedback;
}
}
// 更新机体状态估计
Chassis_UpdateChassisState(c);
return 0;
}
int8_t Chassis_UpdateIMU(Chassis_t *c, const Chassis_IMU_t imu){
if (c == NULL) {
return -1; // 参数错误
}
c->feedback.imu = imu;
// c->feedback.imu.euler.pit = - c->feedback.imu.euler.pit;
return 0;
}
int8_t Chassis_Control(Chassis_t *c, const Chassis_CMD_t *c_cmd){
if (c == NULL || c_cmd == NULL) {
return CHASSIS_ERR_NULL; // 参数错误
}
c->dt = (BSP_TIME_Get_us() - c->lask_wakeup) / 1000000.0f; /* 计算两次调用的时间间隔，单位秒 */
c->lask_wakeup = BSP_TIME_Get_us();
/*设置底盘模式*/
if (Chassis_SetMode(c, c_cmd->mode) != CHASSIS_OK) {
return CHASSIS_ERR_MODE; // 设置模式失败
}
/*根据底盘模式执行不同的控制逻辑*/
switch (c->mode) {
case CHASSIS_MODE_RELAX:
// 放松模式，电机不输出
MOTOR_LZ_Relax(&c->param->joint_motors[0]);
MOTOR_LZ_Relax(&c->param->joint_motors[1]);
MOTOR_LZ_Relax(&c->param->joint_motors[2]);
MOTOR_LZ_Relax(&c->param->joint_motors[3]);
MOTOR_LK_Relax(&c->param->wheel_motors[0]);
MOTOR_LK_Relax(&c->param->wheel_motors[1]);
// 更新VMC正解算用于状态估计
VMC_ForwardSolve(&c->vmc_[0], c->feedback.joint[0].rotor_abs_angle, c->feedback.joint[1].rotor_abs_angle,
c->feedback.imu.euler.pit, c->feedback.imu.gyro.y);
VMC_ForwardSolve(&c->vmc_[1], c->feedback.joint[3].rotor_abs_angle, c->feedback.joint[2].rotor_abs_angle,
c->feedback.imu.euler.pit, c->feedback.imu.gyro.y);
VMC_InverseSolve(&c->vmc_[0], fn, tp);
VMC_GetJointTorques(&c->vmc_[0], &t1, &t2);
// MOTOR_LZ_MotionControl(&c->param->joint_motors[0], &(MOTOR_LZ_MotionParam_t){.torque = t1});
// Chassis_LQRControl(c, c_cmd); // 即使在放松模式下也执行LQR以保持状态更新
break;
case CHASSIS_MODE_RECOVER:
// 恢复模式，使用简单的关节位置控制回到初始姿态
// TODO: 实现恢复逻辑
break;
case CHASSIS_MODE_WHELL_BALANCE:
// 更新VMC正解算用于状态估计
// MOTOR_LZ_Relax(&c->param->joint_motors[0]);
// MOTOR_LZ_Relax(&c->param->joint_motors[1]);
// MOTOR_LZ_Relax(&c->param->joint_motors[2]);
// MOTOR_LZ_Relax(&c->param->joint_motors[3]);
// MOTOR_LK_Relax(&c->param->wheel_motors[0]);
// MOTOR_LK_Relax(&c->param->wheel_motors[1]);
for (int i = 0; i < 4; i++) {
c->output.joint[i].torque = 0.0f;
}
for (int i = 0; i < 2; i++) {
c->output.wheel[i] = 0.0f;
}
// 更新VMC正解算用于状态估计
VMC_ForwardSolve(&c->vmc_[0], c->feedback.joint[0].rotor_abs_angle, c->feedback.joint[1].rotor_abs_angle,
c->feedback.imu.euler.pit, c->feedback.imu.gyro.y);
VMC_ForwardSolve(&c->vmc_[1], c->feedback.joint[3].rotor_abs_angle, c->feedback.joint[2].rotor_abs_angle,
c->feedback.imu.euler.pit, c->feedback.imu.gyro.y);
VMC_InverseSolve(&c->vmc_[0], fn, tp);
VMC_GetJointTorques(&c->vmc_[0], &t1, &t2);
c->output.joint[0].torque = t1;
c->output.joint[1].torque = t2;
// Chassis_LQRControl(c, c_cmd); // 即使在放松模式下也执行LQR以保持状态更新
Chassis_Output(c); // 统一输出
break;
case CHASSIS_MODE_WHELL_LEG_BALANCE:
// 轮腿平衡模式，使用LQR控制
// // 更新VMC正解算
// VMC_ForwardSolve(&c->vmc_[0], c->feedback.joint[0].rotor_abs_angle, c->feedback.joint[1].rotor_abs_angle,
// c->feedback.imu.euler.pit, c->feedback.imu.gyro.y);
// VMC_ForwardSolve(&c->vmc_[1], c->feedback.joint[3].rotor_abs_angle, c->feedback.joint[2].rotor_abs_angle,
// c->feedback.imu.euler.pit, c->feedback.imu.gyro.y);
// // 执行LQR控制
// if (Chassis_LQRControl(c, c_cmd) != 0) {
// // LQR控制失败，切换到安全模式
// return CHASSIS_ERR;
// }
break;
default:
return CHASSIS_ERR_MODE;
}
return CHASSIS_OK;
}
void Chassis_Output(Chassis_t *c) {
if (c == NULL) return;
for (int i = 0; i < 4; i++) {
MOTOR_LZ_MotionParam_t param = {0};
param.torque = c->output.joint[i].torque;
MOTOR_LZ_MotionControl(&c->param->joint_motors[i], &param);
}
for (int i = 0; i < 2; i++) {
MOTOR_LK_SetOutput(&c->param->wheel_motors[i], c->output.wheel[i]);
}
// 这个函数已经在各个模式中直接调用了电机输出函数
// 如果需要统一输出，可以在这里实现
// 现在的设计是在控制逻辑中直接输出，所以这里留空
}

194
User/module/balance_chassis.h
View File

@ -1,194 +0,0 @@
/*
* 平衡底盘模组
*/
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
// Front
// | 1 4 |
// (1)Left| |Right(2)
// | 2 3 |
/* Includes ----------------------------------------------------------------- */
#include <stdint.h>
#include <stdbool.h>
#include "component/vmc.h"
#include "component/lqr.h"
#include "component/ahrs.h"
#include "component/filter.h"
#include "component/pid.h"
#include "device/motor.h"
#include "device/motor_lk.h"
#include "device/motor_lz.h"
/* Exported constants ------------------------------------------------------- */
#define CHASSIS_OK (0) /* 运行正常 */
#define CHASSIS_ERR (-1) /* 运行时发现了其他错误 */
#define CHASSIS_ERR_NULL (-2) /* 运行时发现NULL指针 */
#define CHASSIS_ERR_MODE (-3) /* 运行时配置了错误的CMD_ChassisMode_t */
#define CHASSIS_ERR_TYPE (-4) /* 运行时配置了错误的Chassis_Type_t */
/* Exported macro ----------------------------------------------------------- */
/* Exported types ----------------------------------------------------------- */
typedef enum {
CHASSIS_MODE_RELAX, /* 放松模式，电机不输出。一般情况底盘初始化之后的模式 */
CHASSIS_MODE_RECOVER,
CHASSIS_MODE_WHELL_BALANCE, /* 平衡模式，底盘自我平衡 */
CHASSIS_MODE_WHELL_LEG_BALANCE, /* 轮子+腿平衡模式，底盘自我平衡 */
} Chassis_Mode_t;
typedef struct {
Chassis_Mode_t mode; /* 底盘模式 */
MoveVector_t move_vec; /* 底盘运动向量 */
float height; /* 目标高度 */
} Chassis_CMD_t;
typedef struct {
AHRS_Accl_t accl; /* IMU加速度计 */
AHRS_Gyro_t gyro; /* IMU陀螺仪 */
AHRS_Eulr_t euler; /* IMU欧拉角 */
}Chassis_IMU_t;
typedef struct {
MOTOR_Feedback_t joint[4]; /* 四个关节电机反馈 */
MOTOR_Feedback_t wheel[2]; /* 两个轮子电机反馈 */
Chassis_IMU_t imu; /* IMU数据 */
MOTOR_Feedback_t yaw; /* 云台Yaw轴电机反馈 */
}Chassis_Feedback_t;
typedef struct {
MOTOR_LZ_MotionParam_t joint[4]; /* 四个关节电机输出 */
float wheel[2]; /* 两个轮子电机输出 */
}Chassis_Output_t;
/* 底盘参数的结构体，包含所有初始化用的参数，通常是const，存好几组 */
typedef struct {
VMC_Param_t vmc_param[2]; /* VMC参数 */
LQR_GainMatrix_t lqr_gains; /* LQR增益矩阵 */
MOTOR_LZ_Param_t joint_motors[4]; /* 四个关节电机参数 */
MOTOR_LK_Param_t wheel_motors[2]; /* 两个轮子电机参数 */
float mech_zero_yaw; /* 机械零点 */
/* LQR控制器参数 */
struct {
float max_wheel_torque; /* 轮毂电机最大力矩限制 */
float max_joint_torque; /* 关节电机最大力矩限制 */
} lqr_param;
/* 低通滤波器截止频率 */
struct {
float in; /* 输入 */
float out; /* 输出 */
} low_pass_cutoff_freq;
} Chassis_Params_t;
/*
* 运行的主结构体，所有这个文件里的函数都在操作这个结构体
* 包含了初始化参数，中间变量，输出变量
*/
typedef struct {
uint64_t lask_wakeup;
float dt;
Chassis_Params_t *param; /* 底盘的参数，用Chassis_Init设定 */
AHRS_Eulr_t *mech_zero;
/* 模块通用 */
Chassis_Mode_t mode; /* 底盘模式 */
/* 反馈信息 */
Chassis_Feedback_t feedback;
/* 控制信息*/
Chassis_Output_t output;
VMC_t vmc_[2]; /* 两条腿的VMC */
LQR_Controller_t lqr; /* LQR控制器 */
int8_t state;
float angle;
float height;
/* 机体状态估计 */
struct {
float position_x; /* 机体x位置 */
float velocity_x; /* 机体x速度 */
float last_velocity_x; /* 上一次速度用于数值微分 */
} chassis_state;
float wz_multi; /* 小陀螺模式旋转方向 */
/* PID计算的目标值 */
struct {
AHRS_Eulr_t chassis;
} setpoint;
/* 反馈控制用的PID */
struct {
KPID_t left_wheel; /* 左轮PID */
KPID_t right_wheel; /* 右轮PID */
KPID_t follow; /* 跟随云台用的PID */
KPID_t balance; /* 平衡用的PID */
} pid;
/* 滤波器 */
struct {
LowPassFilter2p_t *in; /* 反馈值滤波器 */
LowPassFilter2p_t *out; /* 输出值滤波器 */
} filter;
} Chassis_t;
/* Exported functions prototypes -------------------------------------------- */
/**
* \brief 初始化底盘
*
* \param c 包含底盘数据的结构体
* \param param 包含底盘参数的结构体指针
* \param target_freq 任务预期的运行频率
*
* \return 函数运行结果
*/
int8_t Chassis_Init(Chassis_t *c, Chassis_Params_t *param, float target_freq);
/**
* \brief 更新底盘的反馈信息
*
* \param c 包含底盘数据的结构体
* \return 函数运行结果
*/
int8_t Chassis_UpdateFeedback(Chassis_t *c);
int8_t Chassis_UpdateIMU(Chassis_t *c, const Chassis_IMU_t imu);
/**
* \brief 运行底盘控制逻辑
*
* \param c 包含底盘数据的结构体
* \param c_cmd 底盘控制指令
*
* \return 函数运行结果
*/
int8_t Chassis_Control(Chassis_t *c, const Chassis_CMD_t *c_cmd);
/**
* \brief 底盘输出值
*
* \param s 包含底盘数据的结构体
* \param out CAN设备底盘输出结构体
*/
void Chassis_Output(Chassis_t *c);
#ifdef __cplusplus
}
#endif

0
User/module/chassis.c
View File

180
User/module/chassis.h
View File

@ -1,180 +0,0 @@
/*
* 底盘模组
*/
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ----------------------------------------------------------------- */
#include "component\cmd.h"
#include "component\filter.h"
#include "component\mixer.h"
#include "component\pid.h"
#include "device\can.h"
#include "device\referee.h"
/* Exported constants ------------------------------------------------------- */
#define CHASSIS_OK (0) /* 运行正常 */
#define CHASSIS_ERR (-1) /* 运行时发现了其他错误 */
#define CHASSIS_ERR_NULL (-2) /* 运行时发现NULL指针 */
#define CHASSIS_ERR_MODE (-3) /* 运行时配置了错误的CMD_ChassisMode_t */
#define CHASSIS_ERR_TYPE (-4) /* 运行时配置了错误的Chassis_Type_t */
/* Exported macro ----------------------------------------------------------- */
/* Exported types ----------------------------------------------------------- */
/* 底盘类型（底盘的物理设计） */
typedef enum {
CHASSIS_TYPE_MECANUM, /* 麦克纳姆轮 */
CHASSIS_TYPE_PARLFIX4, /* 平行摆设的四个驱动轮 */
CHASSIS_TYPE_PARLFIX2, /* 平行摆设的两个驱动轮 */
CHASSIS_TYPE_OMNI_CROSS, /* 叉型摆设的四个全向轮 */
CHASSIS_TYPE_OMNI_PLUS, /* 十字型摆设的四个全向轮 */
CHASSIS_TYPE_DRONE, /* 底盘为无人机 */
CHASSIS_TYPE_SINGLE, /* 单个摩擦轮 */
} Chassis_Type_t;
/* 底盘参数的结构体，包含所有初始化用的参数，通常是const，存好几组 */
typedef struct {
Chassis_Type_t type; /* 底盘类型，底盘的机械设计和轮子选型 */
KPID_Params_t motor_pid_param; /* 轮子控制PID的参数 */
KPID_Params_t follow_pid_param; /* 跟随云台PID的参数 */
/* 低通滤波器截止频率 */
struct {
float in; /* 输入 */
float out; /* 输出 */
} low_pass_cutoff_freq;
/* 电机反装 应该和云台设置相同*/
struct {
bool yaw;
} reverse;
} Chassis_Params_t;
/*
* 运行的主结构体，所有这个文件里的函数都在操作这个结构体
* 包含了初始化参数，中间变量，输出变量
*/
typedef struct {
uint32_t lask_wakeup;
float dt;
const Chassis_Params_t *param; /* 底盘的参数，用Chassis_Init设定 */
AHRS_Eulr_t *mech_zero;
/* 模块通用 */
CMD_ChassisMode_t mode; /* 底盘模式 */
/* 底盘设计 */
int8_t num_wheel; /* 底盘轮子数量 */
Mixer_t mixer; /* 混合器，移动向量->电机目标值 */
MoveVector_t move_vec; /* 底盘实际的运动向量 */
/* 反馈信息 */
struct {
float gimbal_yaw_encoder; /* 云台Yaw轴编码器角度 */
float *motor_rpm; /* 电机转速的动态数组，单位：RPM */
float *motor_current; /* 转矩电流 单位：A */
} feedback;
float wz_multi; /* 小陀螺模式旋转方向 */
/* PID计算的目标值 */
struct {
float *motor_rpm; /* 电机转速的动态数组，单位：RPM */
} setpoint;
/* 反馈控制用的PID */
struct {
KPID_t *motor; /* 控制轮子电机用的PID的动态数组 */
KPID_t follow; /* 跟随云台用的PID */
} pid;
/* 滤波器 */
struct {
LowPassFilter2p_t *in; /* 反馈值滤波器 */
LowPassFilter2p_t *out; /* 输出值滤波器 */
} filter;
float *out; /* 电机最终的输出值的动态数组 */
} Chassis_t;
/* Exported functions prototypes -------------------------------------------- */
/**
* \brief 初始化底盘
*
* \param c 包含底盘数据的结构体
* \param param 包含底盘参数的结构体指针
* \param target_freq 任务预期的运行频率
*
* \return 函数运行结果
*/
int8_t Chassis_Init(Chassis_t *c, const Chassis_Params_t *param,
AHRS_Eulr_t *mech_zero, float target_freq);
/**
* \brief 更新底盘的反馈信息
*
* \param c 包含底盘数据的结构体
* \param can CAN设备结构体
*
* \return 函数运行结果
*/
int8_t Chassis_UpdateFeedback(Chassis_t *c, const CAN_t *can);
/**
* \brief 运行底盘控制逻辑
*
* \param c 包含底盘数据的结构体
* \param c_cmd 底盘控制指令
* \param dt_sec 两次调用的时间间隔
*
* \return 函数运行结果
*/
int8_t Chassis_Control(Chassis_t *c, const CMD_ChassisCmd_t *c_cmd,
uint32_t now);
/**
* @brief 底盘功率限制
*
* @param c 底盘数据
* @param cap 电容数据
* @param ref 裁判系统数据
* @return 函数运行结果
*/
int8_t Chassis_PowerLimit(Chassis_t *c, const CAN_Capacitor_t *cap,
const Referee_ForChassis_t *ref);
/**
* \brief 复制底盘输出值
*
* \param s 包含底盘数据的结构体
* \param out CAN设备底盘输出结构体
*/
void Chassis_DumpOutput(Chassis_t *c, CAN_ChassisOutput_t *out);
/**
* \brief 清空Chassis输出数据
*
* \param out CAN设备底盘输出结构体
*/
void Chassis_ResetOutput(CAN_ChassisOutput_t *out);
/**
* @brief 导出底盘数据
*
* @param chassis 底盘数据结构体
* @param ui UI数据结构体
*/
void Chassis_DumpUI(const Chassis_t *c, Referee_ChassisUI_t *ui);
#ifdef __cplusplus
}
#endif

162
User/module/config.c
View File

@ -1,162 +0,0 @@
/*
* 配置相关
*/
/* Includes ----------------------------------------------------------------- */
#include "module/config.h"
#include "bsp/can.h"
/* Private typedef ---------------------------------------------------------- */
/* Private define ----------------------------------------------------------- */
/* Private macro ------------------------------------------------------------ */
/* Private variables -------------------------------------------------------- */
/* Exported variables ------------------------------------------------------- */
// 机器人参数配置
Config_RobotParam_t robot_config = {
.imu_param = {
.can = BSP_CAN_2,
.can_id = 0x6FF,
.device_id = 0x42,
.master_id = 0x43,
},
.joint_motors[0] = { // 左膝关节
.can = BSP_CAN_1,
.motor_id = 124,
.host_id = 130,
.module = MOTOR_LZ_RSO3,
.reverse = false,
.mode = MOTOR_LZ_MODE_MOTION,
},
.joint_motors[1] = { // 左髋关节
.can = BSP_CAN_1,
.motor_id = 125,
.host_id = 130,
.module = MOTOR_LZ_RSO3,
.reverse = false,
.mode = MOTOR_LZ_MODE_MOTION,
},
.joint_motors[2] = { // 右髋关节
.can = BSP_CAN_1,
.motor_id = 126,
.host_id = 130,
.module = MOTOR_LZ_RSO3,
.reverse = false,
.mode = MOTOR_LZ_MODE_MOTION,
},
.joint_motors[3] = { // 右膝关节
.can = BSP_CAN_1,
.motor_id = 127,
.host_id = 130,
.module = MOTOR_LZ_RSO3,
.reverse = false,
.mode = MOTOR_LZ_MODE_MOTION,
},
.wheel_motors[0] = { // 左轮电机
.can = BSP_CAN_1,
.id = 1, // 电机ID为1，对应命令ID=0x141，反馈ID=0x181
.module = MOTOR_LK_MF9025,
.reverse = false,
},
.wheel_motors[1] = { // 右轮电机
.can = BSP_CAN_1,
.id = 2, // 电机ID为2，对应命令ID=0x142，反馈ID=0x182
.module = MOTOR_LK_MF9025,
.reverse = true,
},
.chassis_param = {
.low_pass_cutoff_freq = {
.in = 30.0f,
.out = 30.0f,
},
.joint_motors = {
{ // 左髋关节
.can = BSP_CAN_1,
.motor_id = 124,
.host_id = 130,
.module = MOTOR_LZ_RSO3,
.reverse = true,
.mode = MOTOR_LZ_MODE_MOTION,
},
{ // 左膝关节
.can = BSP_CAN_1,
.motor_id = 125,
.host_id = 130,
.module = MOTOR_LZ_RSO3,
.reverse = true,
.mode = MOTOR_LZ_MODE_MOTION,
},
{ // 右膝关节
.can = BSP_CAN_1,
.motor_id = 126,
.host_id = 130,
.module = MOTOR_LZ_RSO3,
.reverse = false,
.mode = MOTOR_LZ_MODE_MOTION,
},
{ // 右髋关节
.can = BSP_CAN_1,
.motor_id = 127,
.host_id = 130,
.module = MOTOR_LZ_RSO3,
.reverse = false,
.mode = MOTOR_LZ_MODE_MOTION,
},
},
.wheel_motors = {
{ // 左轮电机
.can = BSP_CAN_1,
.id = 0x141,
.module = MOTOR_LK_MF9025,
.reverse = false,
},
{ // 右轮电机
.can = BSP_CAN_1,
.id = 0x142,
.module = MOTOR_LK_MF9025,
.reverse = true,
},
},
.mech_zero_yaw = 0.0f,
.vmc_param = {
{ // 左腿
.leg_1 = 0.206f, // 前大腿长度 (m)
.leg_2 = 0.258f, // 前小腿长度 (m)
.leg_3 = 0.206f, // 后小腿长度 (m)
.leg_4 = 0.258f, // 后大腿长度 (m)
.hip_length = 0.0f // 髋宽 (m)
},
{ // 右腿
.leg_1 = 0.206f, // 前大腿长度 (m)
.leg_2 = 0.258f, // 前小腿长度 (m)
.leg_3 = 0.206f, // 后小腿长度 (m)
.leg_4 = 0.258f, // 后大腿长度 (m)
.hip_length = 0.0f // 髋宽 (m)
}
},
.lqr_gains ={
.K = {
{ -1.3677, -12.022, 4.0676, -2.6185, -66.132, -4.2516, -1.4083, -0.051404, -57.561, -5.3641 },
{ -1.3677, -12.022, -4.0676, 2.6185, -1.4083, -0.051404, -66.132, -4.2516, -57.561, -5.3641 },
{ 0.14689, 1.2865, -63.224, -12.495, 6.2265, -0.13959, 1.2635, 0.48938, -78.822, -5.121 },
{ 0.14689, 1.2865, 63.224, 12.495, 1.2635, 0.48938, 6.2265, -0.13959, -78.822, -5.121 }
}
},
.lqr_param.max_joint_torque = 20.0f, // 关节电机最大力矩 20Nm
.lqr_param.max_wheel_torque = 2.5f, // 轮毂电机最大力矩 2.5Nm
}
};
/* Private function prototypes ---------------------------------------------- */
/* Exported functions ------------------------------------------------------- */
/**
* @brief 获取机器人配置参数
* @return 机器人配置参数指针
*/
Config_RobotParam_t* Config_GetRobotParam(void) {
return &robot_config;
}

35
User/module/config.h
View File

@ -1,35 +0,0 @@
/*
* 配置相关
*/
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
#include <stdint.h>
#include "device/dm_imu.h"
#include "device/motor_lz.h"
#include "device/motor_lk.h"
#include "module/balance_chassis.h"
typedef struct {
DM_IMU_Param_t imu_param;
MOTOR_LZ_Param_t joint_motors[4];
MOTOR_LK_Param_t wheel_motors[2];
Chassis_Params_t chassis_param;
} Config_RobotParam_t;
/* Exported functions prototypes -------------------------------------------- */
/**
* @brief 获取机器人配置参数
* @return 机器人配置参数指针
*/
Config_RobotParam_t* Config_GetRobotParam(void);
#ifdef __cplusplus
}
#endif

662
User/module/mod_wheelleg_chassis.cpp
View File

@ -1,662 +0,0 @@
#include "mod_wheelleg_chassis.hpp"
#include <cmath>
#include <tuple>
using namespace Module;
Wheelleg::Wheelleg(Param& param)
: param_(param),
roll_pid_(param.roll_pid_param, 333.0f),
yaw_pid_(param.yaw_pid_param, 333.0f),
yaccl_pid_(param.yaccl_pid_param, 333.0f),
lowfilter_(333.0f, 50.0f),
acclfilter_(333.0f, 30.0f),
manfilter_(param.manfilter_param),
ctrl_lock_(true) {
memset(&(this->cmd_), 0, sizeof(this->cmd_));
this->hip_motor_.at(0) =
new Device::MitMotor(param.hip_motor_param.at(0), "hip_left_front");
this->hip_motor_.at(1) =
new Device::MitMotor(param.hip_motor_param.at(1), "hip_left_back");
this->hip_motor_.at(2) =
new Device::MitMotor(param.hip_motor_param.at(2), "hip_right_front");
this->hip_motor_.at(3) =
new Device::MitMotor(param.hip_motor_param.at(3), "hip_left_back");
this->wheel_motor_.at(0) =
new Device::RMMotor(param.wheel_motor_param.at(0), "wheel_left");
this->wheel_motor_.at(1) =
new Device::RMMotor(param.wheel_motor_param.at(1), "wheel_right");
this->leglength_pid_.at(0) =
new Component::PID(param.leglength_pid_param.at(0), 333.0f);
this->leglength_pid_.at(1) =
new Component::PID(param.leglength_pid_param.at(1), 333.0f);
this->theta_pid_.at(0) =
new Component::PID(param.theta_pid_param.at(0), 333.0f);
this->theta_pid_.at(1) =
new Component::PID(param.theta_pid_param.at(1), 333.0f);
this->tp_pid_.at(0) = new Component::PID(param.Tp_pid_param.at(0), 333.0);
this->tp_pid_.at(1) = new Component::PID(param.Tp_pid_param.at(1), 333.0);
this->leg_.at(0) = new Component::VMC(param_.leg_param.at(0), 333.0f);
this->leg_.at(1) = new Component::VMC(param_.leg_param.at(1), 333.0f);
auto event_callback = [](Wheelleg::ChassisEvent event, Wheelleg* chassis) {
chassis->ctrl_lock_.Wait(UINT32_MAX);
switch (event) {
case SET_MODE_RELAX:
chassis->SetMode(RELAX);
break;
case SET_MODE_STAND:
chassis->SetMode(STAND);
break;
case SET_MODE_ROTOR:
chassis->SetMode(ROTOR);
break;
case SET_MODE_RESET:
chassis->SetMode(RESET);
break;
default:
break;
}
chassis->ctrl_lock_.Post();
};
Component::CMD::RegisterEvent<Wheelleg*, ChassisEvent>(
event_callback, this, this->param_.EVENT_MAP);
auto chassis_thread = [](Wheelleg* chassis) {
auto cmd_sub =
Message::Subscriber<Component::CMD::ChassisCMD>("cmd_chassis");
auto eulr_sub =
Message::Subscriber<Component::Type::Eulr>("chassis_imu_eulr");
auto gyro_sub =
Message::Subscriber<Component::Type::Vector3>("chassis_gyro");
auto yaw_sub = Message::Subscriber<float>("chassis_yaw");
auto accl_sub =
Message::Subscriber<Component::Type::Vector3>("chassis_imu_accl_abs");
// auto yaw_sub = Message::Subscriber<float>("chassis_yaw");
uint32_t last_online_time = bsp_time_get_ms();
while (1) {
eulr_sub.DumpData(chassis->eulr_); /* imu */
gyro_sub.DumpData(chassis->gyro_); /* imu */
accl_sub.DumpData(chassis->accl_);
yaw_sub.DumpData(chassis->yaw_);
cmd_sub.DumpData(chassis->cmd_);
// yaw_sub.DumpData(chassis->yaw_);
chassis->ctrl_lock_.Wait(UINT32_MAX);
chassis->MotorSetAble();
chassis->UpdateFeedback();
chassis->Calculate();
chassis->Control();
chassis->ctrl_lock_.Post();
/* 运行结束，等待下一次唤醒 */
chassis->thread_.SleepUntil(3, last_online_time);
}
};
this->thread_.Create(chassis_thread, this, "chassis_thread", 1536,
System::Thread::MEDIUM);
}
void Wheelleg::MotorSetAble() {
if (this->hip_motor_flag_ == 0) {
this->hip_motor_[0]->Relax();
this->hip_motor_[1]->Relax();
this->hip_motor_[2]->Relax();
this->hip_motor_[3]->Relax();
this->dm_motor_flag_ = 0;
}
else {
if (this->dm_motor_flag_ == 0) {
for (int i = 0; i < 5; i++) {
this->hip_motor_[0]->Enable();
}
for (int i = 0; i < 5; i++) {
this->hip_motor_[1]->Enable();
}
for (int i = 0; i < 5; i++) {
this->hip_motor_[2]->Enable();
}
for (int i = 0; i < 5; i++) {
this->hip_motor_[3]->Enable();
}
this->dm_motor_flag_ = 1;
}
};
}
void Wheelleg::UpdateFeedback() {
this->now_ = bsp_time_get();
this->dt_ = TIME_DIFF(this->last_wakeup_, this->now_);
this->last_wakeup_ = this->now_;
this->wheel_motor_[0]->Update();
this->wheel_motor_[1]->Update();
this->leg_argu_[0].phi4_ =
this->hip_motor_[0]->GetAngle() -
this->param_.wheel_param.mech_zero[0]; // 前关节角度
this->leg_argu_[0].phi1_ =
this->hip_motor_[1]->GetAngle() -
this->param_.wheel_param.mech_zero[1]; // 后关节角度
this->leg_argu_[1].phi4_ =
(-this->hip_motor_[2]->GetAngle() +
this->param_.wheel_param.mech_zero[3]); // 前关节角度
if (leg_argu_[1].phi4_ > M_PI) {
this->leg_argu_[1].phi4_ = this->leg_argu_[1].phi4_ - 2 * M_PI;
}
this->leg_argu_[1].phi1_ =
(-this->hip_motor_[3]->GetAngle() +
this->param_.wheel_param.mech_zero[2]); // 后关节角度
if (leg_argu_[1].phi1_ > M_PI) {
this->leg_argu_[1].phi1_ = this->leg_argu_[1].phi1_ - 2 * M_PI;
}
this->pit_ = this->eulr_.pit;
if (this->pit_ > M_PI) {
this->pit_ = this->eulr_.pit - 2 * M_PI;
}
/* 注意极性 */
std::tuple<float, float, float, float> result0 =
this->leg_[0]->VMCsolve(-leg_argu_[0].phi1_, -leg_argu_[0].phi4_,
this->pit_, -this->gyro_.x, this->dt_);
this->leg_argu_[0].L0 = std::get<0>(result0);
this->leg_argu_[0].d_L0 = std::get<1>(result0);
this->leg_argu_[0].theta = -std::get<2>(result0);
this->leg_argu_[0].d_theta = -std::get<3>(result0);
if (leg_argu_[0].theta > M_PI) {
leg_argu_[0].theta = leg_argu_[0].theta - 2 * M_PI;
}
if (leg_argu_[0].theta < -M_PI) {
leg_argu_[0].theta = leg_argu_[0].theta + 2 * M_PI;
}
std::tuple<float, float, float, float> result1 =
this->leg_[1]->VMCsolve(-leg_argu_[1].phi1_, -leg_argu_[1].phi4_,
this->pit_, -this->gyro_.x, this->dt_);
this->leg_argu_[1].L0 = std::get<0>(result1);
this->leg_argu_[1].d_L0 = std::get<1>(result1);
this->leg_argu_[1].theta = -std::get<2>(result1);
this->leg_argu_[1].d_theta = -std::get<3>(result1);
if (leg_argu_[1].theta > M_PI) {
leg_argu_[1].theta = leg_argu_[1].theta - 2 * M_PI;
}
if (leg_argu_[1].theta < -M_PI) {
leg_argu_[1].theta = leg_argu_[1].theta + 2 * M_PI;
}
/* 轮子转速近似于编码器速度 由此推测机体速度 */
this->leg_argu_[0].single_x_dot =
-wheel_motor_[0]->GetSpeed() * 2 * M_PI *
(this->param_.wheel_param.wheel_radius) / 60.f / 15.765 +
leg_argu_[0].L0 * leg_argu_[0].d_theta * cosf(leg_argu_[0].theta) +
leg_argu_[0].d_L0 * sinf(leg_argu_[0].theta);
this->leg_argu_[1].single_x_dot =
wheel_motor_[1]->GetSpeed() * 2 * M_PI *
(this->param_.wheel_param.wheel_radius) / 60.f / 15.765 +
leg_argu_[1].L0 * leg_argu_[1].d_theta * cosf(leg_argu_[1].theta) +
leg_argu_[1].d_L0 * sinf(leg_argu_[1].theta);
this->move_argu_.last_x_dot = this->move_argu_.x_dot;
this->move_argu_.x_dot =
(this->leg_argu_[0].single_x_dot + this->leg_argu_[1].single_x_dot) / 2;
this->move_argu_.x_dot =
(-wheel_motor_[0]->GetSpeed() + wheel_motor_[1]->GetSpeed()) * M_PI *
this->param_.wheel_param.wheel_radius / 60.f / 15.765;
this->move_argu_.x = this->move_argu_.x_dot * dt_ + move_argu_.x;
this->move_argu_.delta_speed =
lowfilter_.Apply((move_argu_.x_dot - move_argu_.last_x_dot) / dt_);
this->move_argu_.accl = acclfilter_.Apply(this->accl_.y * 9.8f);
if (now_ > 1000000) {
this->move_argu_.x_dot_hat =
manfilter_.comp_filter(move_argu_.x_dot, move_argu_.delta_speed,
this->move_argu_.last_x_dot, accl_.y * 9.8f,
dt_) -
((leg_argu_[0].L0 * leg_argu_[0].d_theta * cosf(leg_argu_[0].theta) +
leg_argu_[0].d_L0 * sinf(leg_argu_[0].theta)) +
(leg_argu_[1].L0 * leg_argu_[1].d_theta * cosf(leg_argu_[1].theta) +
leg_argu_[1].d_L0 * sinf(leg_argu_[1].theta))) /
2;
this->move_argu_.xhat = dt_ * move_argu_.x_dot_hat + move_argu_.xhat;
}
}
void Wheelleg::Calculate() {
switch (this->mode_) {
case Wheelleg::RELAX:
// if (fabs(move_argu_.target_dot_x - cmd_.y * 1.5f) > 0.005) {
// if (move_argu_.target_dot_x > cmd_.y * 1.5f) {
// move_argu_.target_dot_x -= 0.004;
// }
// if (move_argu_.target_dot_x < cmd_.y * 1.5f) {
// move_argu_.target_dot_x += 0.004;
// }
// } else {
// move_argu_.target_dot_x = cmd_.y * 1.5f;
// }
// move_argu_.target_x = move_argu_.target_dot_x * dt_ +
// move_argu_.target_x;
move_argu_.target_x = 0;
move_argu_.target_dot_x = 0;
break;
case Wheelleg::STAND:
/* 0.002为最大加速度 即500hz*0.002 梯度式递增减 */
if (fabs(move_argu_.target_dot_x -
cmd_.y * cosf(yaw_) * param_.wheel_param.max_speed) > 0.005) {
if (move_argu_.target_dot_x >
cmd_.y * cosf(yaw_) * param_.wheel_param.max_speed) {
move_argu_.target_dot_x -= 0.004;
}
if (move_argu_.target_dot_x <
cmd_.y * cosf(yaw_) * param_.wheel_param.max_speed) {
move_argu_.target_dot_x += 0.004;
}
} else {
move_argu_.target_dot_x =
cosf(yaw_) * cmd_.y * param_.wheel_param.max_speed;
}
move_argu_.target_x = move_argu_.target_dot_x * dt_ + move_argu_.target_x;
this->move_argu_.target_yaw = 0.0f;
/*双零点*/
if (this->yaw_ > M_PI_2) {
this->move_argu_.target_yaw = 3.14158f;
}
if (this->yaw_ < -M_PI_2) {
this->move_argu_.target_yaw = 3.14158f;
}
break;
case Wheelleg::ROTOR:
if (fabs(move_argu_.target_dot_x -
cmd_.y * cosf(yaw_) * param_.wheel_param.max_speed) > 0.005) {
if (move_argu_.target_dot_x >
cmd_.y * cosf(yaw_) * param_.wheel_param.max_speed) {
move_argu_.target_dot_x -= 0.004;
}
if (move_argu_.target_dot_x <
cmd_.y * cosf(yaw_) * param_.wheel_param.max_speed) {
move_argu_.target_dot_x += 0.004;
}
} else {
move_argu_.target_dot_x =
cosf(yaw_) * cmd_.y * param_.wheel_param.max_speed;
}
move_argu_.target_x = move_argu_.target_dot_x * dt_ + move_argu_.target_x;
this->move_argu_.target_yaw = this->yaw_ + 0.01;
break;
// move_argu_.target_dot_x = cmd_.x;
// move_argu_.target_x =
// move_argu_.target_dot_x * dt_ + move_argu_.target_dot_x;
// // move_argu_.target_dot_x = sqrtf(cmd_.x * cmd_.x + cmd_.y *
// cmd_.y);
// // move_argu_.x_dot =
// // move_argu_.target_dot_x * dt_ + move_argu_.target_dot_x;
// // move_argu_.target_yaw = atan2(cmd_.x, cmd_.y);
// break;
case Wheelleg::RESET:
this->move_argu_.target_dot_x = 0;
move_argu_.target_x = 0;
this->move_argu_.target_yaw = this->eulr_.yaw;
this->move_argu_.xhat = 0;
// move_argu_.target_yaw - atan2(cmd_.x, cmd_.y);
// if ((fabs(move_argu_.target_yaw) - fabs(eulr_.yaw)) > M_PI / 2) {
// this->move_argu_.target_dot_x = -this->move_argu_.target_dot_x;
// }
break;
default:
XB_ASSERT(false);
return;
}
leg_argu_[0].Fn = leg_[0]->GndDetector(leg_argu_[0].Tp, leg_argu_[0].F0);
onground0_flag_ = false;
if (leg_argu_[0].Fn > 30) {
onground0_flag_ = true;
}
leg_argu_[1].Fn = leg_[1]->GndDetector(leg_argu_[1].Tp, leg_argu_[1].F0);
onground1_flag_ = false;
if (leg_argu_[1].Fn > 30) {
onground1_flag_ = true;
}
std::tuple<float, float> result3;
std::tuple<float, float> result4;
switch (this->mode_) {
case Wheelleg::RELAX:
case Wheelleg::ROTOR:
case Wheelleg::STAND:
for (int i = 0; i < 12; i++) {
leg_argu_[0].LQR_K[i] = this->leg_[0]->LQR_K_calc(
&this->param_.wheel_param.K_Poly_Coefficient_L[i][0],
leg_argu_[0].L0);
}
for (int i = 0; i < 12; i++) {
leg_argu_[1].LQR_K[i] = this->leg_[1]->LQR_K_calc(
&this->param_.wheel_param.K_Poly_Coefficient_R[i][0],
leg_argu_[1].L0);
}
if (now_ > 1000000)
if (fabs(move_argu_.target_dot_x) > 0.5 ||
fabs(move_argu_.target_dot_x - move_argu_.x_dot_hat) > 0.7 ||
((!onground0_flag_) & (!onground1_flag_))) {
leg_argu_[0].LQR_K[2] = 0;
leg_argu_[1].LQR_K[2] = 0;
this->move_argu_.xhat = 0;
move_argu_.target_x = 0;
}
if (onground0_flag_) {
leg_argu_[0].Tw =
(leg_argu_[0].LQR_K[0] *
(-0.8845 * leg_argu_[0].L0 + 0.40663 - leg_argu_[0].theta) +
leg_argu_[0].LQR_K[1] * (-leg_argu_[0].d_theta) +
leg_argu_[0].LQR_K[2] *
(-move_argu_.xhat + move_argu_.target_x - 0.56) +
leg_argu_[0].LQR_K[3] *
(-move_argu_.x_dot_hat + move_argu_.target_dot_x) +
leg_argu_[0].LQR_K[4] * (-this->pit_ + 0.16) +
leg_argu_[0].LQR_K[5] * (-this->gyro_.x + 0.0f));
leg_argu_[0].Tp =
(leg_argu_[0].LQR_K[6] *
(-0.8845 * leg_argu_[0].L0 + 0.40663 - leg_argu_[0].theta) +
leg_argu_[0].LQR_K[7] * (-leg_argu_[0].d_theta) +
leg_argu_[0].LQR_K[8] *
(-move_argu_.xhat + move_argu_.target_x - 0.56) +
leg_argu_[0].LQR_K[9] *
(-move_argu_.x_dot_hat + move_argu_.target_dot_x) +
leg_argu_[0].LQR_K[10] * (-this->pit_ + 0.16) +
leg_argu_[0].LQR_K[11] * (-this->gyro_.x + 0.0f));
} else {
leg_argu_[0].Tw = 0;
leg_argu_[0].Tp =
(leg_argu_[0].LQR_K[6] * (-leg_argu_[0].theta + 0.0f) +
leg_argu_[0].LQR_K[7] * (-leg_argu_[0].d_theta + 0.0f));
}
if (onground1_flag_) {
leg_argu_[1].Tw =
(leg_argu_[1].LQR_K[0] *
(-0.8845 * leg_argu_[1].L0 + 0.40663 - leg_argu_[1].theta) +
leg_argu_[1].LQR_K[1] * (-leg_argu_[1].d_theta + 0.0f) +
leg_argu_[1].LQR_K[2] *
(-move_argu_.xhat + move_argu_.target_x - 0.56) +
leg_argu_[1].LQR_K[3] *
(-move_argu_.x_dot_hat + move_argu_.target_dot_x) +
leg_argu_[1].LQR_K[4] * (-this->pit_ + 0.16) +
leg_argu_[1].LQR_K[5] * (-this->gyro_.x + 0.0f));
leg_argu_[1].Tp =
(leg_argu_[1].LQR_K[6] *
(-0.8845 * leg_argu_[1].L0 + 0.40663 - leg_argu_[1].theta) +
leg_argu_[1].LQR_K[7] * (-leg_argu_[1].d_theta + 0.0f) +
leg_argu_[1].LQR_K[8] *
(-move_argu_.xhat + move_argu_.target_x - 0.56) +
leg_argu_[1].LQR_K[9] *
(-move_argu_.x_dot_hat + move_argu_.target_dot_x) +
leg_argu_[1].LQR_K[10] * (-this->pit_ + 0.16) +
leg_argu_[1].LQR_K[11] * (-this->gyro_.x + 0.0f));
} else {
leg_argu_[1].Tw = 0;
leg_argu_[1].Tp =
(leg_argu_[1].LQR_K[6] * (-leg_argu_[1].theta + 0.0f) +
leg_argu_[1].LQR_K[7] * (-leg_argu_[1].d_theta + 0.0f));
}
this->leg_argu_[0].Delat_L0 = fabs(0.35 * cmd_.z) - fabs(gyro_.z) / 100 +
this->param_.wheel_param.static_L0[0] +
+roll_pid_.Calculate(0, eulr_.rol, dt_);
this->leg_argu_[1].Delat_L0 = fabs(0.35 * cmd_.z) - fabs(gyro_.z) / 100 +
this->param_.wheel_param.static_L0[1] -
roll_pid_.Calculate(0, eulr_.rol, dt_);
leg_argu_[0].F0 =
leg_argu_[0].Tp * sinf(leg_argu_[0].theta) +
this->param_.wheel_param.static_F0[0] +
leglength_pid_.at(0)->Calculate(this->leg_argu_[0].Delat_L0,
this->leg_argu_[0].L0, this->dt_);
leg_argu_[1].F0 =
leg_argu_[1].Tp * sinf(leg_argu_[1].theta) +
this->param_.wheel_param.static_F0[1] +
leglength_pid_.at(1)->Calculate(this->leg_argu_[1].Delat_L0,
this->leg_argu_[1].L0, this->dt_);
this->leg_argu_[0].Delta_Tp =
leg_argu_[0].L0 * 10.0f *
tp_pid_.at(0)->Calculate(this->leg_argu_[1].theta,
this->leg_argu_[0].theta, this->dt_);
this->leg_argu_[1].Delta_Tp =
-leg_argu_[1].L0 * 10.0f *
tp_pid_.at(0)->Calculate(this->leg_argu_[1].theta,
this->leg_argu_[0].theta, this->dt_);
result3 = leg_[0]->VMCinserve(
-leg_argu_[0].phi1_, -leg_argu_[0].phi4_,
-leg_argu_[0].Tp - this->leg_argu_[0].Delta_Tp, leg_argu_[0].F0);
this->leg_argu_[0].T1 = std::get<0>(result3);
this->leg_argu_[0].T2 = std::get<1>(result3);
result4 = leg_[1]->VMCinserve(
-leg_argu_[1].phi1_, -leg_argu_[1].phi4_,
-leg_argu_[1].Tp - this->leg_argu_[1].Delta_Tp, leg_argu_[1].F0);
this->leg_argu_[1].T1 = -std::get<0>(result4);
this->leg_argu_[1].T2 = -std::get<1>(result4);
if (onground0_flag_ & onground1_flag_) {
move_argu_.yaw_force =
-this->yaw_pid_.Calculate(move_argu_.target_yaw, this->yaw_, dt_);
} else {
move_argu_.yaw_force = 0;
}
/*3508不带减速箱是Tw*3.2f*/
/*2006带减速是Tw/1.8f*/
/* 3508带15.7减速箱是Tw/4.9256 */
this->wheel_motor_out_[0] =
this->leg_argu_[0].Tw / 4.9256f - move_argu_.yaw_force;
this->wheel_motor_out_[1] =
this->leg_argu_[1].Tw / 4.9256f + move_argu_.yaw_force;
this->hip_motor_out_[0] = this->leg_argu_[0].T1;
this->hip_motor_out_[1] = this->leg_argu_[0].T2;
this->hip_motor_out_[2] = this->leg_argu_[1].T1;
this->hip_motor_out_[3] = this->leg_argu_[1].T2;
this->hip_motor_flag_ = 1;
break;
case Wheelleg::RESET:
if (fabs(pit_) > M_PI / 2 || fabs(leg_argu_[0].theta) > 1.57 ||
fabs(leg_argu_[1].theta) > 1.57) {
leg_argu_[0].target_theta = leg_argu_[0].theta + cmd_.y / 1000;
if (fabs(pit_) > M_PI / 2) {
if ((leg_argu_[0].theta - leg_argu_[1].theta) > 0.03) {
leg_argu_[1].target_theta = leg_argu_[1].theta + 0.001;
}
if ((leg_argu_[0].theta - leg_argu_[1].theta) < -0.03) {
leg_argu_[1].target_theta = leg_argu_[1].theta - 0.001;
}
leg_argu_[0].F0 = 10 * leglength_pid_.at(0)->Calculate(
0.65f, this->leg_argu_[0].L0, this->dt_);
leg_argu_[1].F0 = 10 * leglength_pid_.at(1)->Calculate(
0.65f, this->leg_argu_[1].L0, this->dt_);
}
if (fabs(leg_argu_[0].theta) < 1.57) {
leg_argu_[1].target_theta = leg_argu_[1].theta + cmd_.y / 1000;
leg_argu_[0].target_theta = leg_argu_[0].theta;
}
if (fabs(leg_argu_[1].theta) < 1.57) {
leg_argu_[0].target_theta = leg_argu_[0].theta + cmd_.y / 1000;
leg_argu_[1].target_theta = leg_argu_[1].theta;
}
if (leg_argu_[0].target_theta > M_PI) {
leg_argu_[0].target_theta -= 2 * M_PI;
}
if (leg_argu_[0].target_theta < -M_PI) {
leg_argu_[0].target_theta += 2 * M_PI;
}
if (leg_argu_[1].target_theta > M_PI) {
leg_argu_[1].target_theta -= 2 * M_PI;
}
if (leg_argu_[1].target_theta < -M_PI) {
leg_argu_[1].target_theta += 2 * M_PI;
}
leg_argu_[0].Tp =
500 * this->theta_pid_[0]->Calculate(leg_argu_[0].target_theta,
leg_argu_[0].theta, dt_);
leg_argu_[1].Tp =
500 * this->theta_pid_[1]->Calculate(leg_argu_[1].target_theta,
leg_argu_[1].theta, dt_);
} else {
leg_argu_[0].F0 = 3 * leglength_pid_.at(0)->Calculate(
0.10f, this->leg_argu_[0].L0, this->dt_);
leg_argu_[1].F0 = 3 * leglength_pid_.at(1)->Calculate(
0.10f, this->leg_argu_[1].L0, this->dt_);
if ((this->leg_argu_[0].L0 < 0.20) && (this->leg_argu_[1].L0 < 0.20)) {
leg_argu_[0].Tp = 5.5 * this->theta_pid_[0]->Calculate(
0.1, leg_argu_[0].theta, dt_);
leg_argu_[1].Tp = 5.5 * this->theta_pid_[1]->Calculate(
0.1, leg_argu_[1].theta, dt_);
clampf(&leg_argu_[0].Tp, -10, 10);
clampf(&leg_argu_[1].Tp, -10, 10);
} else {
leg_argu_[0].Tp = 0;
leg_argu_[1].Tp = 0;
}
}
result3 = leg_[0]->VMCinserve(-leg_argu_[0].phi1_, -leg_argu_[0].phi4_,
-leg_argu_[0].Tp, leg_argu_[0].F0);
this->leg_argu_[0].T1 = std::get<0>(result3);
this->leg_argu_[0].T2 = std::get<1>(result3);
result4 = leg_[1]->VMCinserve(-leg_argu_[1].phi1_, -leg_argu_[1].phi4_,
-leg_argu_[1].Tp, leg_argu_[1].F0);
this->leg_argu_[1].T1 = -std::get<0>(result4);
this->leg_argu_[1].T2 = -std::get<1>(result4);
this->hip_motor_out_[0] = this->leg_argu_[0].T1;
this->hip_motor_out_[1] = this->leg_argu_[0].T2;
this->hip_motor_out_[2] = this->leg_argu_[1].T1;
this->hip_motor_out_[3] = this->leg_argu_[1].T2;
this->hip_motor_flag_ = 1;
break;
}
}
void Wheelleg::Control() {
clampf(&wheel_motor_out_[0], -0.8f, 0.8f);
clampf(&wheel_motor_out_[1], -0.8f, 0.8f);
clampf(&hip_motor_out_[0], -20.0f, 20.0f);
clampf(&hip_motor_out_[1], -20.0f, 20.0f);
clampf(&hip_motor_out_[2], -20.0f, 20.0f);
clampf(&hip_motor_out_[3], -20.0f, 20.0f);
// if (fabs(wheel_motor_[0]->GetSpeed()) > 5000 ||
// fabs(wheel_motor_[1]->GetSpeed()) > 5000) {
// wheel_motor_out_[0] = 0;
// wheel_motor_out_[1] = 0;
// if (fabs(this->pit_) > 0.5f) {
// this->hip_motor_flag_ = 0;
// }
// }
switch (this->mode_) {
case Wheelleg::RELAX:
this->wheel_motor_[0]->Relax();
this->wheel_motor_[1]->Relax();
hip_motor_flag_ = 0;
hip_motor_[0]->SetMit(0.0f);
hip_motor_[1]->SetMit(0.0f);
hip_motor_[2]->SetMit(0.0f);
hip_motor_[3]->SetMit(0.0f);
break;
case Wheelleg::STAND:
case Wheelleg::ROTOR:
// this->wheel_motor_[0]->Relax();
// this->wheel_motor_[1]->Relax();
this->wheel_motor_[0]->Control(-wheel_motor_out_[0]);
this->wheel_motor_[1]->Control(wheel_motor_out_[1]);
hip_motor_[0]->SetMit(this->hip_motor_out_[0]);
hip_motor_[1]->SetMit(this->hip_motor_out_[1]);
hip_motor_[2]->SetMit(this->hip_motor_out_[2]);
hip_motor_[3]->SetMit(this->hip_motor_out_[3]);
// hip_motor_[0]->SetMit(0.0f);
// hip_motor_[1]->SetMit(0.0f);
// hip_motor_[2]->SetMit(0.0f);
// hip_motor_[3]->SetMit(0.0f);
break;
case Wheelleg::RESET:
this->wheel_motor_[0]->Relax();
this->wheel_motor_[1]->Relax();
hip_motor_[0]->SetMit(this->hip_motor_out_[0]);
hip_motor_[1]->SetMit(this->hip_motor_out_[1]);
hip_motor_[2]->SetMit(this->hip_motor_out_[2]);
hip_motor_[3]->SetMit(this->hip_motor_out_[3]);
break;
}
}
void Wheelleg::SetMode(Wheelleg::Mode mode) {
if (mode == this->mode_) {
return;
}
this->leg_[0]->Reset();
this->leg_[1]->Reset();
move_argu_.x = 0;
move_argu_.x_dot = 0;
move_argu_.last_x_dot = 0;
move_argu_.target_x = move_argu_.xhat;
move_argu_.target_yaw = this->eulr_.yaw;
move_argu_.target_dot_x = 0;
move_argu_.xhat = 0;
move_argu_.x_dot_hat = 0;
this->manfilter_.reset_comp();
this->mode_ = mode;
}

57
User/task/atti_esti.c
View File

@ -1,57 +0,0 @@
/*
atti_esti Task
*/
/* Includes ----------------------------------------------------------------- */
#include "task/user_task.h"
/* USER INCLUDE BEGIN */
#include "bsp/can.h"
#include "device/dm_imu.h"
#include "module/config.h"
#include "module/balance_chassis.h"
/* USER INCLUDE END */
/* Private typedef ---------------------------------------------------------- */
/* Private define ----------------------------------------------------------- */
/* Private macro ------------------------------------------------------------ */
/* Private variables -------------------------------------------------------- */
/* USER STRUCT BEGIN */
DM_IMU_t dm_imu;
Chassis_IMU_t chassis_imu_send;
/* USER STRUCT END */
/* Private function --------------------------------------------------------- */
/* Exported functions ------------------------------------------------------- */
void Task_atti_esti(void *argument) {
(void)argument; /* 未使用argument，消除警告 */
/* 计算任务运行到指定频率需要等待的tick数 */
const uint32_t delay_tick = osKernelGetTickFreq() / ATTI_ESTI_FREQ;
osDelay(ATTI_ESTI_INIT_DELAY); /* 延时一段时间再开启任务 */
uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */
/* USER CODE INIT BEGIN */
BSP_CAN_Init();
// 初始化DM IMU设备
DM_IMU_Init(&dm_imu, &Config_GetRobotParam()->imu_param);
/* USER CODE INIT END */
while (1) {
tick += delay_tick; /* 计算下一个唤醒时刻 */
/* USER CODE BEGIN */
if (DM_IMU_AutoUpdateAll(&dm_imu) == DEVICE_OK) {
osMessageQueueReset(task_runtime.msgq.chassis_imu); // 重置消息队列，防止阻塞
chassis_imu_send.accl = dm_imu.data.accl;
chassis_imu_send.gyro = dm_imu.data.gyro;
chassis_imu_send.euler = dm_imu.data.euler;
osMessageQueuePut(task_runtime.msgq.chassis_imu, &chassis_imu_send, 0, 0); // 非阻塞发送IMU数据
}
/* USER CODE END */
osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */
}
}

50
User/task/blink.c
View File

@ -1,50 +0,0 @@
/*
blink Task
*/
/* Includes ----------------------------------------------------------------- */
#include "task/user_task.h"
/* USER INCLUDE BEGIN */
#include "bsp/pwm.h"
#include <math.h>
/* USER INCLUDE END */
/* Private typedef ---------------------------------------------------------- */
/* Private define ----------------------------------------------------------- */
/* Private macro ------------------------------------------------------------ */
/* Private variables -------------------------------------------------------- */
/* USER STRUCT BEGIN */
/* USER STRUCT END */
/* Private function --------------------------------------------------------- */
/* Exported functions ------------------------------------------------------- */
void Task_blink(void *argument) {
(void)argument; /* 未使用argument，消除警告 */
/* 计算任务运行到指定频率需要等待的tick数 */
const uint32_t delay_tick = osKernelGetTickFreq() / BLINK_FREQ;
osDelay(BLINK_INIT_DELAY); /* 延时一段时间再开启任务 */
uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */
/* USER CODE INIT BEGIN */
BSP_PWM_Stop(BSP_PWM_LED_R);
BSP_PWM_Stop(BSP_PWM_LED_B);
BSP_PWM_SetComp(BSP_PWM_LED_G, 0.0f);
BSP_PWM_Start(BSP_PWM_LED_G);
/* USER CODE INIT END */
while (1) {
tick += delay_tick; /* 计算下一个唤醒时刻 */
/* USER CODE BEGIN */
// 呼吸灯 - 基于tick的正弦波
float duty = (sinf(tick * 0.003f) + 1.0f) * 0.5f; // 0到1之间的正弦波，加快频率
BSP_PWM_SetComp(BSP_PWM_LED_G, duty);
/* USER CODE END */
osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */
}
}

28
User/task/config.yaml
View File

@ -1,28 +0,0 @@
- delay: 0
description: ''
freq_control: true
frequency: 100.0
function: Task_blink
name: blink
stack: 256
- delay: 0
description: ''
freq_control: true
frequency: 500.0
function: Task_rc
name: rc
stack: 256
- delay: 0
description: ''
freq_control: true
frequency: 1000.0
function: Task_atti_esti
name: atti_esti
stack: 256
- delay: 500
description: ''
freq_control: true
frequency: 500.0
function: Task_ctrl_chassis
name: ctrl_chassis
stack: 512

73
User/task/ctrl_chassis.c
View File

@ -1,73 +0,0 @@
/*
ctrl_chassis Task
*/
/* Includes ----------------------------------------------------------------- */
#include "task/user_task.h"
/* USER INCLUDE BEGIN */
#include "bsp/can.h"
#include "device/motor_lk.h"
#include "device/motor_lz.h"
#include "module/config.h"
#include "module/balance_chassis.h"
/* USER INCLUDE END */
/* Private typedef ---------------------------------------------------------- */
/* Private define ----------------------------------------------------------- */
/* Private macro ------------------------------------------------------------ */
/* Private variables -------------------------------------------------------- */
/* USER STRUCT BEGIN */
Chassis_t chassis;
Chassis_CMD_t chassis_cmd = {
.mode = CHASSIS_MODE_RECOVER, // 改为RECOVER模式，让电机先启动
.move_vec = {
.vx = 0.0f,
.vy = 0.0f,
.wz = 0.0f,
},
.height = 0.0f,
};
Chassis_IMU_t chassis_imu;
MOTOR_Feedback_t left_wheel_fb;
MOTOR_Feedback_t right_wheel_fb;
/* USER STRUCT END */
/* Private function --------------------------------------------------------- */
/* Exported functions ------------------------------------------------------- */
void Task_ctrl_chassis(void *argument) {
(void)argument; /* 未使用argument，消除警告 */
/* 计算任务运行到指定频率需要等待的tick数 */
const uint32_t delay_tick = osKernelGetTickFreq() / CTRL_CHASSIS_FREQ;
osDelay(CTRL_CHASSIS_INIT_DELAY); /* 延时一段时间再开启任务 */
uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */
/* USER CODE INIT BEGIN */
Chassis_Init(&chassis, &Config_GetRobotParam()->chassis_param, CTRL_CHASSIS_FREQ);
/* USER CODE INIT END */
while (1) {
tick += delay_tick; /* 计算下一个唤醒时刻 */
/* USER CODE BEGIN */
if (osMessageQueueGet(task_runtime.msgq.chassis_imu, &chassis_imu, NULL, 0) == osOK) {
chassis.feedback.imu = chassis_imu;
}
if (osMessageQueueGet(task_runtime.msgq.Chassis_cmd, &chassis_cmd, NULL, 0) == osOK) {
// 成功接收到命令，更新底盘命令
}
Chassis_UpdateFeedback(&chassis);
Chassis_Control(&chassis, &chassis_cmd);
/* USER CODE END */
osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */
}}

49
User/task/init.c
View File

@ -1,49 +0,0 @@
/*
Init Task
任务初始化，创建各个线程任务和消息队列
*/
/* Includes ----------------------------------------------------------------- */
#include "task/user_task.h"
/* USER INCLUDE BEGIN */
#include "component/ahrs.h"
#include "module/config.h"
#include "module/balance_chassis.h"
/* USER INCLUDE BEGIN */
/* Private typedef ---------------------------------------------------------- */
/* Private define ----------------------------------------------------------- */
/* Private macro ------------------------------------------------------------ */
/* Private variables -------------------------------------------------------- */
/* Private function --------------------------------------------------------- */
/* Exported functions ------------------------------------------------------- */
/**
* \brief 初始化
*
* \param argument 未使用
*/
void Task_Init(void *argument) {
(void)argument; /* 未使用argument，消除警告 */
/* USER CODE INIT BEGIN */
/* USER CODE INIT END */
osKernelLock(); /* 锁定内核，防止任务切换 */
/* 创建任务线程 */
task_runtime.thread.blink = osThreadNew(Task_blink, NULL, &attr_blink);
task_runtime.thread.rc = osThreadNew(Task_rc, NULL, &attr_rc);
task_runtime.thread.atti_esti = osThreadNew(Task_atti_esti, NULL, &attr_atti_esti);
task_runtime.thread.ctrl_chassis = osThreadNew(Task_ctrl_chassis, NULL, &attr_ctrl_chassis);
// 创建消息队列
/* USER MESSAGE BEGIN */
task_runtime.msgq.user_msg= osMessageQueueNew(2u, 10, NULL);
task_runtime.msgq.chassis_imu = osMessageQueueNew(2u, sizeof(Chassis_IMU_t), NULL);
task_runtime.msgq.Chassis_cmd = osMessageQueueNew(2u, sizeof(Chassis_CMD_t), NULL);
/* USER MESSAGE END */
osKernelUnlock(); // 解锁内核
osThreadTerminate(osThreadGetId()); // 任务完成后结束自身
}

90
User/task/rc.c
View File

@ -1,90 +0,0 @@
/*
rc Task
*/
/* Includes ----------------------------------------------------------------- */
#include "task/user_task.h"
/* USER INCLUDE BEGIN */
#include "device/dr16.h"
#include "module/balance_chassis.h"
/* USER INCLUDE END */
/* Private typedef ---------------------------------------------------------- */
/* Private define ----------------------------------------------------------- */
/* Private macro ------------------------------------------------------------ */
/* Private variables -------------------------------------------------------- */
/* USER STRUCT BEGIN */
DR16_t dr16;
Chassis_CMD_t cmd_to_chassis;
/* USER STRUCT END */
/* Private function --------------------------------------------------------- */
/* Exported functions ------------------------------------------------------- */
void Task_rc(void *argument) {
(void)argument; /* 未使用argument，消除警告 */
/* 计算任务运行到指定频率需要等待的tick数 */
const uint32_t delay_tick = osKernelGetTickFreq() / RC_FREQ;
osDelay(RC_INIT_DELAY); /* 延时一段时间再开启任务 */
uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */
/* USER CODE INIT BEGIN */
DR16_Init(&dr16);
/* USER CODE INIT END */
while (1) {
tick += delay_tick; /* 计算下一个唤醒时刻 */
/* USER CODE BEGIN */
DR16_StartDmaRecv(&dr16);
if (DR16_WaitDmaCplt(20)) {
// 根据左拨杆设置底盘模式
switch (dr16.data.sw_l) {
case 1: // 上位
cmd_to_chassis.mode = CHASSIS_MODE_RELAX;
break;
case 3: // 中位
cmd_to_chassis.mode = CHASSIS_MODE_RELAX;
break;
case 2: // 下位
cmd_to_chassis.mode = CHASSIS_MODE_WHELL_BALANCE;
break;
default:
cmd_to_chassis.mode = CHASSIS_MODE_RELAX;
break;
}
// 解析遥控器摇杆数据为运动向量
// 将遥控器通道值从[364, 1684]映射到[-1.0, 1.0]
float ch_l_y_norm = (float)(dr16.data.ch_l_y - 1024) / 660.0f; // 前后
float ch_l_x_norm = (float)(dr16.data.ch_l_x - 1024) / 660.0f; // 左右
float ch_r_x_norm = (float)(dr16.data.ch_r_y - 1024) / 660.0f; // 旋转
// 设置运动向量（根据需要调整增益）
cmd_to_chassis.move_vec.vx = ch_l_y_norm * 2.0f; // 前后运动，增益可调
cmd_to_chassis.move_vec.vy = ch_l_x_norm * 2.0f; // 左右运动，增益可调
cmd_to_chassis.move_vec.wz = ch_r_x_norm * 3.0f; // 旋转运动，增益可调
// 设置目标高度（可根据右拨杆或其他输入调整）
cmd_to_chassis.height = dr16.data.res-1024; // 目标高度，范围[-1024, 1024]，可根据需要调整比例
// 发送命令到底盘控制任务
osMessageQueuePut(task_runtime.msgq.Chassis_cmd, &cmd_to_chassis, 0, 0);
} else {
// 超时处理，发送安全命令
cmd_to_chassis.mode = CHASSIS_MODE_RELAX;
cmd_to_chassis.move_vec.vx = 0.0f;
cmd_to_chassis.move_vec.vy = 0.0f;
cmd_to_chassis.move_vec.wz = 0.0f;
cmd_to_chassis.height = 0.0f;
osMessageQueuePut(task_runtime.msgq.Chassis_cmd, &cmd_to_chassis, 0, 0);
}
/* USER CODE END */
osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */
}
}

31
User/task/user_task.c
View File

@ -1,31 +0,0 @@
#include "task/user_task.h"
Task_Runtime_t task_runtime;
const osThreadAttr_t attr_init = {
.name = "Task_Init",
.priority = osPriorityRealtime,
.stack_size = 256 * 4,
};
/* User_task */
const osThreadAttr_t attr_blink = {
.name = "blink",
.priority = osPriorityNormal,
.stack_size = 256 * 4,
};
const osThreadAttr_t attr_rc = {
.name = "rc",
.priority = osPriorityNormal,
.stack_size = 256 * 4,
};
const osThreadAttr_t attr_atti_esti = {
.name = "atti_esti",
.priority = osPriorityNormal,
.stack_size = 256 * 4,
};
const osThreadAttr_t attr_ctrl_chassis = {
.name = "ctrl_chassis",
.priority = osPriorityNormal,
.stack_size = 512 * 4,
};

108
User/task/user_task.h
View File

@ -1,108 +0,0 @@
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ----------------------------------------------------------------- */
#include <cmsis_os2.h>
#include "FreeRTOS.h"
#include "task.h"
/* USER INCLUDE BEGIN */
/* USER INCLUDE END */
/* Exported constants ------------------------------------------------------- */
/* 任务运行频率 */
#define BLINK_FREQ (100.0)
#define RC_FREQ (500.0)
#define ATTI_ESTI_FREQ (1000.0)
#define CTRL_CHASSIS_FREQ (500.0)
/* 任务初始化延时ms */
#define TASK_INIT_DELAY (100u)
#define BLINK_INIT_DELAY (0)
#define RC_INIT_DELAY (0)
#define ATTI_ESTI_INIT_DELAY (0)
#define CTRL_CHASSIS_INIT_DELAY (500)
/* Exported defines --------------------------------------------------------- */
/* Exported macro ----------------------------------------------------------- */
/* Exported types ----------------------------------------------------------- */
/* 任务运行时结构体 */
typedef struct {
/* 各任务，也可以叫做线程 */
struct {
osThreadId_t blink;
osThreadId_t rc;
osThreadId_t atti_esti;
osThreadId_t ctrl_chassis;
} thread;
/* USER MESSAGE BEGIN */
struct {
osMessageQueueId_t user_msg; /* 用户自定义任务消息队列 */
osMessageQueueId_t chassis_imu;
osMessageQueueId_t Chassis_cmd;
} msgq;
/* USER MESSAGE END */
/* 机器人状态 */
struct {
float battery; /* 电池电量百分比 */
float vbat; /* 电池电压 */
float cpu_temp; /* CPU温度 */
} status;
/* USER CONFIG BEGIN */
/* USER CONFIG END */
/* 各任务的stack使用 */
struct {
UBaseType_t blink;
UBaseType_t rc;
UBaseType_t atti_esti;
UBaseType_t ctrl_chassis;
} stack_water_mark;
/* 各任务运行频率 */
struct {
float blink;
float rc;
float atti_esti;
float ctrl_chassis;
} freq;
/* 任务最近运行时间 */
struct {
float blink;
float rc;
float atti_esti;
float ctrl_chassis;
} last_up_time;
} Task_Runtime_t;
/* 任务运行时结构体 */
extern Task_Runtime_t task_runtime;
/* 初始化任务句柄 */
extern const osThreadAttr_t attr_init;
extern const osThreadAttr_t attr_blink;
extern const osThreadAttr_t attr_rc;
extern const osThreadAttr_t attr_atti_esti;
extern const osThreadAttr_t attr_ctrl_chassis;
/* 任务函数声明 */
void Task_Init(void *argument);
void Task_blink(void *argument);
void Task_rc(void *argument);
void Task_atti_esti(void *argument);
void Task_ctrl_chassis(void *argument);
#ifdef __cplusplus
}
#endif

244
User/整合方案说明.md
View File

@ -1,244 +0,0 @@
# 轮腿机器人控制系统整合方案
## 📋 现有系统分析
### 🏗️ 您的系统架构（优势）
1. **成熟的硬件接口**：已实现HT+BM电机CAN通信
2. **完整的运动学**：四连杆正/逆运动学 + 雅可比矩阵
3. **丰富的控制模式**：平衡、运动、跳跃、悬浮检测
4. **详细的参数数据**：基于实际测量的腿部参数表
5. **MATLAB设计工具**：完整的LQR参数生成流程
### ⚠️ 可以改进的地方
1. **控制逻辑分散**：LQR、VMC、MPC混合使用，逻辑复杂
2. **参数管理困难**：大量硬编码参数分散在各处
3. **缺乏统一接口**：不同控制算法接口不一致
## 🚀 整合方案
### 方案一：渐进式整合（推荐）
#### 第一阶段：保持现有系统，添加新接口
```c
// 在Chassis_Task.c中添加
#include "integrated_balance_control.h"
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);
// 新增：整合控制器调用
integrated_chassis_control_update();
// 如果使用新控制器，跳过原有的力矩计算
if (!get_current_controller_type()) {
Chassis_Torque_Calculation(&chassis_control);
Chassis_Torque_Combine(&chassis_control);
}
Motor_CMD_Send(&chassis_control);
vTaskDelay(1);
}
}
```
#### 第二阶段：参数统一管理
创建统一的参数配置文件：
```c
// robot_config.h
#ifndef ROBOT_CONFIG_H
#define ROBOT_CONFIG_H
// 机器人物理参数（与MATLAB一致）
#define ROBOT_R_W 0.09f // 驱动轮半径 (m)
#define ROBOT_R_L 0.25f // 轮距/2 (m)
#define ROBOT_L_C 0.037f // 机体质心到关节距离 (m)
#define ROBOT_M_W 0.8f // 驱动轮质量 (kg)
#define ROBOT_M_L 1.6183599f // 腿部质量 (kg)
#define ROBOT_M_B 11.542f // 机体质量 (kg)
// 腿部几何参数（与您的定义一致）
#define LEG_L1 0.15f // 大腿长度 (m)
#define LEG_L2 0.25f // 小腿长度 (m)
#define LEG_L3 0.25f //
#define LEG_L4 0.15f //
#define LEG_L5 0.1f // 髋关节间距/2 (m)
// 控制参数
#define CONTROL_FREQUENCY 500.0f // 控制频率 (Hz)
#define MAX_WHEEL_TORQUE 50.0f // 最大轮子力矩 (N*m)
#define MAX_JOINT_TORQUE 18.0f // 最大关节力矩 (N*m)
// 安全参数
#define MAX_TILT_ANGLE 0.25f // 最大倾斜角 (rad)
#define LEG_LENGTH_MIN 0.11f // 最小腿长 (m)
#define LEG_LENGTH_MAX 0.30f // 最大腿长 (m)
#endif
```
#### 第三阶段：控制器选择接口
添加动态切换功能：
```c
// 在遥控器中添加切换功能
void handle_controller_switch(const Gimbal_ctrl_t* rc_ctrl)
{
static uint8_t last_switch_state = 0;
uint8_t current_switch_state = 0; // 从遥控器读取开关状态
if (current_switch_state != last_switch_state) {
if (current_switch_state == 1) {
// 切换到新控制器
set_controller_type(1);
} else {
// 切换到原有控制器
set_controller_type(0);
}
last_switch_state = current_switch_state;
}
}
```
### 方案二：完全替换（激进）
直接用新的balance_control替换现有的控制逻辑：
#### 修改Chassis_Task主循环
```c
void Chassis_Task(void const * argument)
{
vTaskDelay(CHASSIS_TASK_INIT_TIME);
// 初始化新的平衡控制器
balance_controller_t balance_ctrl;
balance_control_init(&balance_ctrl, TASK_RUN_TIME);
vTaskDelay(100);
while(1)
{
// 数据更新（保留）
Chassis_Data_Update(&chassis_control);
// 转换数据格式
imu_data_t imu_data;
motor_feedback_t motor_fb;
control_command_t cmd;
convert_chassis_to_balance_imu(&chassis_control, &imu_data);
convert_chassis_to_balance_motor_fb(&chassis_control, &motor_fb);
convert_chassis_to_balance_cmd(&chassis_control, &cmd);
// 使用新控制器
balance_control_update(&balance_ctrl, &imu_data, &motor_fb, &cmd);
// 获取控制输出
motor_control_t motor_ctrl;
get_motor_control_output(&balance_ctrl, &motor_ctrl);
// 转换回原有格式并发送
convert_balance_to_chassis_output(&motor_ctrl, &chassis_control);
Motor_CMD_Send(&chassis_control);
vTaskDelay(1);
}
}
```
## 🔧 具体修改步骤
### 1. 保留您的优势代码
- **保留**：`Forward_kinematic_solution()` - 运动学计算
- **保留**：`Supportive_Force_Cal()` - 支撑力计算
- **保留**：`CAN_HT_CMD()` / `CAN_BM_CONTROL_CMD()` - 电机通信
- **保留**：MATLAB LQR设计流程
### 2. 整合控制架构
- **替换**：分散的LQR/VMC控制逻辑 → 统一的balance_control
- **简化**：复杂的状态机 → 清晰的模式管理
- **统一**：参数管理 → robot_config.h
### 3. 数据接口适配
```c
// 现有的HT电机数据
typedef struct {
const HT_motor_measure_t *motor_measure;
fp32 position;
fp32 velocity;
fp32 torque_out, torque_get;
} joint_motor_t;
// 转换为新系统格式
void convert_ht_motor_to_balance_fb(const joint_motor_t* ht_motor,
float* angle, float* velocity, float* torque)
{
*angle = ht_motor->position;
*velocity = ht_motor->velocity;
*torque = ht_motor->torque_get;
}
```
### 4. 参数迁移
将您MATLAB中的参数直接用于新系统：
```c
// 使用您的LQR拟合系数
const float K_fit_coefficients[40][6] = {
// 复制您MATLAB生成的系数
// ...
};
// 使用您的机器人参数
#define R_W_ACTUAL 0.09f // 与您的R_w_ac一致
#define R_L_ACTUAL 0.25f // 与您的R_l_ac一致
// ...
```
## 🎯 推荐的实施步骤
### 第1周：准备工作
1. 备份现有代码
2. 创建新的git分支
3. 编译并测试现有系统确保正常
### 第2周：集成准备
1. 添加integrated_balance_control文件
2. 修改Chassis_Task添加切换接口
3. 保持原有控制器为默认
### 第3周：参数对齐
1. 创建robot_config.h统一参数
2. 运行MATLAB脚本生成LQR系数
3. 对比调试确保参数一致
### 第4周：功能测试
1. 在安全环境下测试新控制器
2. 对比两套控制器的性能
3. 调试和优化
### 第5周：全面切换
1. 将新控制器设为默认
2. 移除冗余的旧代码
3. 文档整理和代码规范化
## 🛡️ 风险控制
1. **渐进式切换**：保留原有系统作为备份
2. **参数验证**：MATLAB仿真验证参数正确性
3. **安全测试**：在安全环境下测试新控制器
4. **性能对比**：记录并对比控制效果
5. **回滚机制**：确保可以快速回到原有系统
这样您既可以利用现有系统的优势，又能获得新架构的清晰性和可维护性。您觉得哪个方案更适合您的需求？

0
d435.py
View File

171
some_functions.c
View File

@ -1,171 +0,0 @@
// 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 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 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;
}
}

321
utils/lqr.asv
View File

@ -1,321 +0,0 @@
% 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.77; % 驱动轮半径 （单位：m）
R_l_ac = 0.210; % 两个驱动轮之间距离/2 （单位：m）
l_c_ac = 0.025; % 机体质心到腿部关节中心点距离 （单位：m）
m_w_ac = 0.5; m_l_ac = 2.133; m_b_ac = 4.542; % 驱动轮质量 腿部质量 机体质量 （单位：kg）
I_w_ac = (7630000)*10^(-7); % 驱动轮转动惯量 （单位：kg m^2）
I_b_ac = 0.3470; % 机体转动惯量(自然坐标系法向) （单位：kg m^2）
I_z_ac = 0.322; % 机器人z轴转动惯量 （单位：kg m^2）
%%%%%%%%%%%%%%%%%%%%%%机器人腿部参数（定腿长调试用）%%%%%%%%%%%%%%%%%%%%%%%%
% 如果需要使用此部分，请去掉120-127行以及215-218行注释，然后将224行之后的所有代码注释掉
% 或者点击左侧数字"224"让程序只运行行之前的内容并停止
l_l_ac = 0.16; % 左腿摆杆长度 （左腿对应数据） （单位：m）
l_wl_ac = 0.10; % 左驱动轮质心到左腿摆杆质心距离 （单位：m）
l_bl_ac = 0.4; % 机体转轴到左腿摆杆质心距离 （单位：m）
I_ll_ac = 0.01886; % 左腿摆杆转动惯量 （单位：kg m^2）
l_r_ac = 0.16; % 右腿摆杆长度 （右腿对应数据） （单位：m）
l_wr_ac = 0.10; % 右驱动轮质心到右腿摆杆质心距离 （单位：m）
l_br_ac = 0.4; % 机体转轴到右腿摆杆质心距离 （单位：m）
I_lr_ac = 0.01886; % 右腿摆杆转动惯量 （单位：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

321
utils/lqr.m
View File

@ -1,321 +0,0 @@
% 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.77; % 驱动轮半径 （单位：m）
R_l_ac = 0.210; % 两个驱动轮之间距离/2 （单位：m）
l_c_ac = 0.025; % 机体质心到腿部关节中心点距离 （单位：m）
m_w_ac = 0.5; m_l_ac = 2.133; m_b_ac = 4.542; % 驱动轮质量 腿部质量 机体质量 （单位：kg）
I_w_ac = (7630000)*10^(-7); % 驱动轮转动惯量 （单位：kg m^2）
I_b_ac = 0.3470; % 机体转动惯量(自然坐标系法向) （单位：kg m^2）
I_z_ac = 0.322; % 机器人z轴转动惯量 （单位：kg m^2）
%%%%%%%%%%%%%%%%%%%%%%机器人腿部参数（定腿长调试用）%%%%%%%%%%%%%%%%%%%%%%%%
% 如果需要使用此部分，请去掉120-127行以及215-218行注释，然后将224行之后的所有代码注释掉
% 或者点击左侧数字"224"让程序只运行行之前的内容并停止
l_l_ac = 0.16; % 左腿摆杆长度 （左腿对应数据） （单位：m）
l_wl_ac = 0.10; % 左驱动轮质心到左腿摆杆质心距离 （单位：m）
l_bl_ac = 0.4; % 机体转轴到左腿摆杆质心距离 （单位：m）
I_ll_ac = 0.01886; % 左腿摆杆转动惯量 （单位：kg m^2）
l_r_ac = 0.16; % 右腿摆杆长度 （右腿对应数据） （单位：m）
l_wr_ac = 0.10; % 右驱动轮质心到右腿摆杆质心距离 （单位：m）
l_br_ac = 0.4; % 机体转轴到右腿摆杆质心距离 （单位：m）
I_lr_ac = 0.01886; % 右腿摆杆转动惯量 （单位：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

86
utils/matlab/LQR_calc.m
View File

@ -1,86 +0,0 @@
clear;
clc;
syms x xd xdd T Tp thetadd thetad theta phidd phid phi P N PM NM L LM;
%% 参数设定
% 均为标准单位制
g = 9.81;
% 驱动轮
R = 0.075; %轮子半径
mw = 0.58; %轮子质量
Iw = 0.00823; %轮子转动惯量
% 大腿
l_active_leg = 0.14;
m_active_leg = 0.174;
% 小腿
l_slave_leg = 0.24;
m_slave_leg = 0.180;
% 关节间距
joint_distance = 0.11;
% 摆杆
mp = (m_active_leg + m_slave_leg)*2 + 0.728; % 需要加上定子质量
Ip = mp*L^2/3; %摆杆转动惯量
% 机体
M = 12; %机体重量
l = -0.014; %机体质心到关节电机转轴的距离
IM = 0.124;
%% 经典力学方程
fu1=N-NM==mp*(xdd+L*(thetadd*cos(theta)-thetad*thetad*sin(theta)));
fu2=P-PM-mp*g==mp*L*(-thetadd*sin(theta)-thetad*thetad*cos(theta));
fu3=NM==M*(xdd+(L+LM)*(thetadd*cos(theta)-thetad*thetad*sin(theta))-l*(phidd*cos(phi)-phid*phid*sin(phi)));
fu4=PM-M*g==M*((L+LM)*(-thetadd*sin(theta)-thetad*thetad*cos(theta))+l*(-phidd*sin(phi)-phid*phid*cos(phi)));
%% 不同部件之间的力求解
[N,NM,P,PM]=solve(fu1,fu2,fu3,fu4,N,NM,P,PM);
f1=xdd==(T-N*R)/(Iw/R+mw*R);
f2=Ip*thetadd==(P*L+PM*LM)*sin(theta)-(N*L+NM*LM)*cos(theta)-T+Tp;
f3=IM*phidd==Tp+NM*l*cos(phi)+PM*l*sin(phi);
[xdd,thetadd,phidd]=solve(f1,f2,f3,xdd,thetadd,phidd);
%% 计算雅可比矩阵A and B
func=[thetad,thetadd,xd,xdd,phid,phidd];
A_lin_model=jacobian(func,[theta,thetad,x,xd,phi,phid]);
temp_A=subs(A_lin_model,[theta,thetad,xd,phi,phid],zeros(1,5));
final_A=simplify(temp_A);
B_lin_model=jacobian(func,[T Tp]);
temp_B=subs(B_lin_model,[theta,thetad,xd,phi,phid],zeros(1,5));
final_B=simplify(temp_B);
%% 计算不同腿长下LQR增益K
L_var = 0.1; % 腿质心到机体转轴距离
Q_mat = diag([1,1,500,100,5000,1]);
R_mat = diag([1,0.25]);
K = zeros(20,12);
leg_len = zeros(20,1);
for i=1:20
L_var = L_var + 0.005;
leg_len(i) = L_var*2;
A = double(subs(final_A, [L LM], [L_var L_var]));
B = double(subs(final_B, [L LM], [L_var L_var]));
KK = lqrd(A, B, Q_mat, R_mat, 0.001);
KK_t=KK.';
K(i,:)=KK_t(:);
end
%% 不同腿长下二项式拟合K
K_cons=zeros(12,3);
for i=1:12
res=fit(leg_len,K(:,i),"poly2");
K_cons(i,:)=[res.p1, res.p2, res.p3];
end
for j=1:12
for i=1:3
fprintf("%f,",K_cons(j,i));
end
fprintf("\n");
end

103
utils/matlab/UseBodyVel.m
View File

@ -1,103 +0,0 @@
clear;
clc;
syms theta phi L x x_b N N_f T T_p M N_M P_M L_M
syms theta_dot x_dot phi_dot theta_ddot x_ddot phi_ddot
syms x_b_dot x_b_ddot
%% 参数设定
% 均为标准单位制
g = 9.81; %重力加速度
% 驱动轮
mw = 0.58; %轮子质量
R = 0.075; %轮子半径
Iw = 0.00823; %轮子转动惯量
% 大腿
l_active_leg = 0.14;
m_active_leg = 0.174;
% 小腿
l_slave_leg = 0.24;
m_slave_leg = 0.180;
% 关节间距
joint_distance = 0.11;
% 摆杆
mp = (m_active_leg + m_slave_leg)*2 + 0.728;
Ip = mp*L^2/3; % 摆杆转动惯量
% 机体
M = 12; %机体重量
IM = 0.124; %机体惯量,绕质心
l = -0.014; %机体质心到电机转轴的距离
% QR设置为相同的权重
Q_cost = diag([1,1,500,100,5000,1]);
R_cost = diag([1,0.25]);
useBodyVelocity = 1;
%% 经典力学方程
if useBodyVelocity
x_ddot = x_b_ddot - (L+L_M)*cos(theta)*theta_ddot+ (L+L_M)*sin(theta)*theta_dot^2;
end
N_M = M*(x_ddot+(L+L_M)*theta_ddot*cos(theta)-(L+L_M)*theta_dot^2*sin(theta)-l*phi_ddot*cos(phi)+l*phi_dot^2*sin(phi));
P_M = M*(g-(L+L_M)*theta_ddot*sin(theta)-(L+L_M)*theta_dot^2*cos(theta)-l*phi_ddot*sin(phi)-l*phi_dot^2*cos(phi));
N = mp*(x_ddot+L*theta_ddot*cos(theta)-L*theta_dot^2*sin(theta))+N_M;
P = mp*(g-L*theta_dot^2*cos(theta)-L*theta_ddot*sin(theta))+P_M;
eqA = x_ddot == (T-N*R)/(Iw/R+mw*R);
eqB = Ip*theta_ddot == (P*L+P_M*L_M)*sin(theta)-(N*L+N_M*L_M)*cos(theta) - T + T_p;
eqC = IM*phi_ddot == T_p + N_M*l*cos(phi) + P_M*l*sin(phi);
%% 计算雅可比矩阵
U = [T T_p].';
if useBodyVelocity
model_sol = solve([eqA eqB eqC],[theta_ddot,x_b_ddot,phi_ddot]);
X = [theta,theta_dot,x_b,x_b_dot,phi,phi_dot].';
dX = [theta_dot,simplify(model_sol.theta_ddot),...
x_b_dot,simplify(model_sol.x_b_ddot),...
phi_dot,simplify(model_sol.phi_ddot)].';
A_sym = subs(jacobian(dX,X),[theta theta_dot x_b_dot phi phi_dot],zeros(1,5));
B_sym = subs(jacobian(dX,U),[theta theta_dot x_b_dot phi phi_dot],zeros(1,5));
else
model_sol = solve([eqA eqB eqC],[theta_ddot,x_ddot,phi_ddot]);
X = [theta,theta_dot,x,x_dot,phi,phi_dot].';
dX = [theta_dot,simplify(model_sol.theta_ddot),...
x_dot,simplify(model_sol.x_ddot),...
phi_dot,simplify(model_sol.phi_ddot)].';
A_sym = subs(jacobian(dX,X),[theta theta_dot x_dot phi phi_dot],zeros(1,5));
B_sym = subs(jacobian(dX,U),[theta theta_dot x_dot phi phi_dot],zeros(1,5));
end
%% 计算变长度LQR
L_var = 0.1; % 腿质心到机体转轴距离
K=zeros(20,12);
leglen=zeros(20,1);
for i=1:20
L_var=L_var+0.005; % 10mm线性化一次
leglen(i)=L_var*2;
trans_A=double(subs(A_sym,[L L_M],[L_var L_var]));
trans_B=double(subs(B_sym,[L L_M],[L_var L_var]));
KK=lqrd(trans_A,trans_B,Q_cost,R_cost,0.001);
KK_t=KK.';
K(i,:)=KK_t(:);
end
%% 用二项式拟合K,一共12个参数
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
for j=1:12
for i=1:3
fprintf("%f,",K_cons(j,i));
end
fprintf("\n");
end

39
utils/matlab/vmc.m
View File

@ -1,39 +0,0 @@
clear;
clc;
syms phi1(t) phi2(t) phi3(t) phi4(t) l5 phi0 L0 T_Leg F_Leg
syms phi_dot_1 phi_dot_4
syms l1 l2 l3 l4
%% 腿长解算
x_B = l1*cos(phi1);
y_B = l1*sin(phi1);
x_C = x_B+l2*cos(phi2);
y_C = y_B+l2*sin(phi2);
x_D = l5+l4*cos(phi4);
y_D = l4*sin(phi4);
x_dot_B = diff(x_B,t);
y_dot_B = diff(y_B,t);
x_dot_C = diff(x_C,t);
y_dot_C = diff(y_C,t);
x_dot_D = diff(x_D,t);
y_dot_D = diff(y_D,t);
%% 速度导数求解
phi_dot_2 = ((x_dot_D-x_dot_B)*cos(phi3)+(y_dot_D-y_dot_B)*sin(phi3))/l2/sin(phi3-phi2);
x_dot_C = subs(x_dot_C,diff(phi2,t),phi_dot_2);
x_dot_C = subs(x_dot_C,[diff(phi1,t),diff(phi4,t)],[phi_dot_1,phi_dot_4]);
y_dot_C = subs(y_dot_C,diff(phi2,t),phi_dot_2);
y_dot_C = subs(y_dot_C,[diff(phi1,t),diff(phi4,t)],[phi_dot_1,phi_dot_4]);
%% 运动映射(虚功原理)+极坐标转换
x_dot = [x_dot_C; y_dot_C];
q_dot = [phi_dot_1; phi_dot_4];
x_dot = collect(simplify(collect(x_dot,q_dot)),q_dot);
J = simplify(jacobian(x_dot,q_dot));
rotate = [cos(phi0-pi/2) -sin(phi0-pi/2);
sin(phi0-pi/2) cos(phi0-pi/2)];
map = [0 -1/L0;
1 0];
Trans_Jacobian = simplify(J.'*rotate*map);

BIN
utils/matlab/wheel_leg.slx
View File

Binary file not shown.

BIN
utils/matlab/wheel_leg.slxc
View File

Binary file not shown.

219
utils/vmc.m
View File

@ -1,219 +0,0 @@
function [tau_hip, tau_knee] = vmc_control(L1, L2, q1, q2, F_virtual, theta_virtual)
% VMC (Virtual Model Control) for serial leg mechanism
% 基于虚拟模型控制的串联腿机构力矩计算
%
% 输入参数:
% L1: 大腿长度 (m)
% L2: 小腿长度 (m)
% q1: 髋关节角度 (rad)
% q2: 膝关节角度 (rad)
% F_virtual: 虚拟力大小 (N)
% theta_virtual: 虚拟力方向角度 (rad)
%
% 输出:
% tau_hip: 髋关节所需力矩 (N*m)
% tau_knee: 膝关节所需力矩 (N*m)
% 计算雅可比矩阵
J = compute_jacobian(L1, L2, q1, q2);
% 虚拟力向量 (笛卡尔空间)
F_cartesian = [F_virtual * cos(theta_virtual);
F_virtual * sin(theta_virtual)];
% 通过雅可比转置将笛卡尔力转换为关节力矩
tau = J' * F_cartesian;
tau_hip = tau(1);
tau_knee = tau(2);
end
function J = compute_jacobian(L1, L2, q1, q2)
% 计算串联腿机构的雅可比矩阵
% 从关节空间到足端笛卡尔空间的映射
%
% 输入参数:
% L1: 大腿长度 (m)
% L2: 小腿长度 (m)
% q1: 髋关节角度 (rad)
% q2: 膝关节角度 (rad)
%
% 输出:
% J: 2x2雅可比矩阵
% 足端位置 (相对于髋关节)
% x = L1*cos(q1) + L2*cos(q1+q2)
% y = L1*sin(q1) + L2*sin(q1+q2)
% 雅可比矩阵元素
J11 = -L1*sin(q1) - L2*sin(q1+q2); % dx/dq1
J12 = -L2*sin(q1+q2); % dx/dq2
J21 = L1*cos(q1) + L2*cos(q1+q2); % dy/dq1
J22 = L2*cos(q1+q2); % dy/dq2
J = [J11, J12;
J21, J22];
end
function [x, y] = forward_kinematics(L1, L2, q1, q2)
% 串联腿正运动学 - 足端位置
%
% 输入参数:
% L1: 大腿长度 (m)
% L2: 小腿长度 (m)
% q1: 髋关节角度 (rad)
% q2: 膝关节角度 (rad)
%
% 输出:
% x: 足端x坐标 (m)
% y: 足端y坐标 (m)
x = L1*cos(q1) + L2*cos(q1+q2);
y = L1*sin(q1) + L2*sin(q1+q2);
end
function [q1, q2] = inverse_kinematics(L1, L2, x, y)
% 串联腿逆运动学 - 关节角度
%
% 输入参数:
% L1: 大腿长度 (m)
% L2: 小腿长度 (m)
% x: 足端目标x坐标 (m)
% y: 足端目标y坐标 (m)
%
% 输出:
% q1: 髋关节角度 (rad)
% q2: 膝关节角度 (rad)
% 计算到足端的距离
r = sqrt(x^2 + y^2);
% 检查工作空间
if r > (L1 + L2) || r < abs(L1 - L2)
error('目标位置超出工作空间范围');
end
% 使用余弦定理计算膝关节角度
cos_q2 = (r^2 - L1^2 - L2^2) / (2*L1*L2);
q2 = acos(cos_q2); % 肘部向上的解
% 计算髋关节角度
alpha = atan2(y, x);
beta = atan2(L2*sin(q2), L1 + L2*cos(q2));
q1 = alpha - beta;
end
function [angle_equiv, length_equiv] = serial_to_virtual_leg(L1, L2, q1, q2)
% 将串联腿转换为等效摆动杆表示 (对应C代码中的正运动学)
%
% 输入参数:
% L1: 大腿长度 (m)
% L2: 小腿长度 (m)
% q1: 髋关节角度 (rad)
% q2: 膝关节角度 (rad)
%
% 输出:
% angle_equiv: 等效摆动杆角度 (rad)
% length_equiv: 等效摆动杆长度 (m)
% 对应C代码中的计算
q4 = (pi - q1 - q2) / 2;
% 使用余弦定理求解等效摆动杆长度
a = 1.0;
b = -2.0 * L1 * cos(q4);
c = L1^2 - L2^2;
discriminant = b^2 - 4*a*c;
if discriminant < 0
error('无实数解，配置不可达');
end
sqrt_discriminant = sqrt(discriminant);
L3_1 = (-b + sqrt_discriminant) / (2*a);
L3_2 = (-b - sqrt_discriminant) / (2*a);
% 选择正的解
if L3_1 > 0 && L3_2 > 0
L3 = min(L3_1, L3_2); % 选择较小的解
elseif L3_1 > 0
L3 = L3_1;
elseif L3_2 > 0
L3 = L3_2;
else
error('无正解');
end
angle_equiv = q1 + q4;
length_equiv = L3;
end
function [q1, q2] = virtual_to_serial_leg(L1, L2, angle_equiv, length_equiv)
% 将等效摆动杆转换为串联腿表示 (对应C代码中的逆运动学)
%
% 输入参数:
% L1: 大腿长度 (m)
% L2: 小腿长度 (m)
% angle_equiv: 等效摆动杆角度 (rad)
% length_equiv: 等效摆动杆长度 (m)
%
% 输出:
% q1: 髋关节角度 (rad)
% q2: 膝关节角度 (rad)
h = length_equiv;
q = angle_equiv;
% 计算cos(q4)
cos_q4 = (L1^2 + h^2 - L2^2) / (2.0 * L1 * h);
% 检查范围
if cos_q4 < -1.0 || cos_q4 > 1.0
error('不可达配置');
end
q4 = acos(cos_q4);
% 计算关节角度
q1 = q - q4;
q2 = pi - 2.0 * q4 - q1;
end
% 示例使用函数
function demo_vmc_control()
% VMC控制示例
% 腿部参数
L1 = 0.15; % 大腿长度 15cm
L2 = 0.15; % 小腿长度 15cm
% 当前关节角度
q1 = pi/6; % 30度
q2 = pi/3; % 60度
% 期望的虚拟力
F_virtual = 50; % 50N
theta_virtual = -pi/2; % 向下的力
% 计算所需关节力矩
[tau_hip, tau_knee] = vmc_control(L1, L2, q1, q2, F_virtual, theta_virtual);
fprintf('髋关节所需力矩: %.3f N*m\n', tau_hip);
fprintf('膝关节所需力矩: %.3f N*m\n', tau_knee);
% 显示雅可比矩阵
J = compute_jacobian(L1, L2, q1, q2);
fprintf('雅可比矩阵:\n');
disp(J);
% 测试等效摆动杆转换
[angle_eq, length_eq] = serial_to_virtual_leg(L1, L2, q1, q2);
fprintf('等效摆动杆角度: %.3f rad (%.1f deg)\n', angle_eq, rad2deg(angle_eq));
fprintf('等效摆动杆长度: %.3f m\n', length_eq);
% 验证逆变换
[q1_back, q2_back] = virtual_to_serial_leg(L1, L2, angle_eq, length_eq);
fprintf('验证 - 原始角度: q1=%.3f, q2=%.3f\n', q1, q2);
fprintf('验证 - 恢复角度: q1=%.3f, q2=%.3f\n', q1_back, q2_back);
end