Compare commits
8 Commits
23ae0c3fa9
...
58f7ba86db
| Author | SHA1 | Date | |
|---|---|---|---|
| 58f7ba86db | |||
| e7aaf1b98f | |||
| b7876f7eab | |||
| 6bb0e96aa8 | |||
| 38d4139f06 | |||
| 9574929545 | |||
| e75094a41d | |||
| 843ca3d931 |
1358
Chassis_Task.c
Normal file
1358
Chassis_Task.c
Normal file
File diff suppressed because it is too large
Load Diff
299
Chassis_Task.h
Normal file
299
Chassis_Task.h
Normal file
@ -0,0 +1,299 @@
|
||||
#ifndef _CHASSIS_TASK
|
||||
#define _CHASSIS_TASK
|
||||
|
||||
#include "main.h"
|
||||
#include "struct_typedef.h"
|
||||
#include "pid.h"
|
||||
#include "bsp_can.h"
|
||||
// ------------- Limit info -------------
|
||||
#define MAX_ACCL 13000.0f
|
||||
#define MAX_ACCL_JOINT 15.0f
|
||||
#define MAX_FOOT_OUTPUT 2048
|
||||
|
||||
// ------------- Target value info -------------
|
||||
#define SIT_MODE_HEIGHT_SET 0.18f
|
||||
#define NORMAL_MODE_HEIGHT_SET 0.10f
|
||||
#define HIGH_MODE_HEIGHT_SET 0.27f
|
||||
#define EXTREMELY_HIGH_MODE_HEIGHT_SET 0.30f
|
||||
|
||||
// ------------- Mech info -------------
|
||||
#define L1 0.15f
|
||||
#define L2 0.25f
|
||||
#define L3 0.25f
|
||||
#define L4 0.15f
|
||||
#define L5 0.1f
|
||||
|
||||
#define WHEEL_PERIMETER 0.56547
|
||||
#define WHEEL_RADIUS 0.09f
|
||||
#define LEG_OFFSET 0.3790855135f // 水平位置到上限位的夹角
|
||||
#define LOWER_SUPPORT_FORCE_FOR_JUMP 5.0f
|
||||
#define LOWER_SUPPORT_FORCE 0.0f
|
||||
#define MOVE_LOWER_BOUND 0.3f
|
||||
#define EXIT_PITCH_ANGLE 0.1f
|
||||
#define DANGER_PITCH_ANGLE 0.25f
|
||||
|
||||
#define FEED_f 7.5f
|
||||
#define FEED_f_1 3.5f
|
||||
|
||||
#define NORMAL_MODE_WEIGHT_DISTANCE_OFFSET -0.0f
|
||||
|
||||
#define MOTOR_POS_UPPER_BOUND 0.05f
|
||||
#define MOTOR_POS_LOWER_BOUND 1.4f
|
||||
#define LIMITED_TORQUE 0.5f
|
||||
#define UNLIMITED_TORQUE 200.0f
|
||||
|
||||
// ------------- Time info -------------
|
||||
#define CHASSIS_TASK_INIT_TIME 500
|
||||
#define TASK_RUN_TIME 0.002f
|
||||
|
||||
// ------------- Transfer info -------------
|
||||
#define MOTOR_ECD_TO_RAD 0.00019174779 // 2*PI / 32767
|
||||
#define HALF_ECD_RANGE 14383
|
||||
#define HALF_POSITION_RANGE 3.0f
|
||||
// #define CC 0.00512f
|
||||
// #define CC 1/494.0f
|
||||
#define TORQ_K 494.483818182
|
||||
// ------------- Math info -------------
|
||||
#define PI2 6.28318530717959f
|
||||
#define PI 3.14159265358979f
|
||||
#define PI_2 1.57079632679489f
|
||||
#define PI_4 0.78539816339744f
|
||||
|
||||
|
||||
|
||||
typedef enum
|
||||
{
|
||||
ENABLE_CHASSIS = 0,
|
||||
DISABLE_CHASSIS,
|
||||
} chassis_mode_e;
|
||||
|
||||
typedef enum
|
||||
{
|
||||
NO_FORCE,
|
||||
FOOT_LAUNCHING,
|
||||
JOINT_LAUNCHING,
|
||||
BALANCING_READY,
|
||||
JOINT_REDUCING,
|
||||
} chassis_balancing_mode_e;
|
||||
|
||||
typedef enum
|
||||
{
|
||||
NONE,
|
||||
NORMAL_MOVING_MODE,
|
||||
ABNORMAL_MOVING_MODE,
|
||||
JUMPING_MODE,
|
||||
CAP_MODE,
|
||||
FLY_MODE,
|
||||
TK_MODE,
|
||||
} sport_mode_e;
|
||||
|
||||
typedef enum
|
||||
{
|
||||
READY_TO_JUMP,
|
||||
PREPARING_LANDING,
|
||||
PREPARING_STAND_JUMPING,
|
||||
CONSTACTING_LEGS,
|
||||
EXTENDING_LEGS,
|
||||
CONSTACTING_LEGS_2,
|
||||
FINISHED,
|
||||
} jumping_stage_e;
|
||||
|
||||
typedef enum
|
||||
{
|
||||
NOT_DEFINE,
|
||||
STANDING_JUMP,
|
||||
MOVING_JUMP,
|
||||
} jumping_mode_e;
|
||||
|
||||
typedef enum
|
||||
{
|
||||
SIT_MODE = 0,
|
||||
NORMAL_MODE,
|
||||
HIGH_MODE,
|
||||
EXTREMELY_HIGH_MODE,
|
||||
CHANGING_HIGH,
|
||||
} chassis_high_mode_e;
|
||||
|
||||
typedef enum
|
||||
{
|
||||
MOTOR_NO_FORCE = 0,
|
||||
MOTOR_FORCE,
|
||||
} chassis_motor_mode_e;
|
||||
|
||||
typedef enum
|
||||
{
|
||||
ON_GROUND = 0,
|
||||
OFF_GROUND = 1,
|
||||
} suspend_flag_e;
|
||||
|
||||
typedef struct
|
||||
{
|
||||
chassis_mode_e chassis_mode, last_chassis_mode;
|
||||
chassis_balancing_mode_e chassis_balancing_mode, last_chassis_balancing_mode;
|
||||
sport_mode_e sport_mode, last_sport_mode;
|
||||
|
||||
jumping_mode_e jumping_mode, last_jumping_mode;
|
||||
jumping_stage_e jumping_stage, last_jumping_stage;
|
||||
|
||||
chassis_high_mode_e chassis_high_mode, last_chassis_high_mode;
|
||||
|
||||
} mode_t;
|
||||
|
||||
typedef struct
|
||||
{
|
||||
const fp32 *chassis_INS_angle_point;
|
||||
const fp32 *chassis_INS_gyro_point;
|
||||
const fp32 *chassis_INS_accel_point;
|
||||
fp32 yaw_angle, pitch_angle, roll_angle;
|
||||
fp32 yaw_gyro, pitch_gyro, roll_gyro;
|
||||
fp32 yaw_accel, pitch_accel, roll_accel;
|
||||
|
||||
fp32 yaw_angle_sett, pitch_angle_set, roll_angle_set;
|
||||
fp32 yaw_gyro_set, pitch_gyro_set, roll_gyro_set;
|
||||
|
||||
fp32 ideal_high;
|
||||
fp32 leg_length_L, last_leg_length_L, leg_length_L_set;
|
||||
fp32 leg_length_R, last_leg_length_R, leg_length_R_set;
|
||||
fp32 leg_dlength_L;
|
||||
fp32 leg_dlength_R;
|
||||
|
||||
fp32 foot_roll_angle;
|
||||
fp32 leg_angle_L, last_leg_angle_L, leg_angle_L_set;
|
||||
fp32 leg_angle_R, last_leg_angle_R, leg_angle_R_set;
|
||||
fp32 leg_gyro_L, leg_gyro_R;
|
||||
|
||||
fp32 foot_distance, foot_distance_K, foot_distance_set;
|
||||
fp32 foot_speed, foot_speed_K, foot_speed_set;
|
||||
|
||||
fp32 supportive_force_L, supportive_force_R;
|
||||
|
||||
} chassis_posture_info_t;
|
||||
|
||||
typedef struct
|
||||
{
|
||||
// -------- horizontal force --------
|
||||
fp32 joint_balancing_torque_L, joint_balancing_torque_R;
|
||||
fp32 foot_balancing_torque_L, foot_balancing_torque_R;
|
||||
|
||||
fp32 foot_moving_torque_L, foot_moving_torque_R;
|
||||
fp32 joint_moving_torque_L, joint_moving_torque_R;
|
||||
|
||||
fp32 joint_prevent_splits_torque_L, joint_prevent_splits_torque_R;
|
||||
|
||||
fp32 joint_horizontal_torque_L, joint_horizontal_torque_R;
|
||||
fp32 foot_horizontal_torque_L, foot_horizontal_torque_R;
|
||||
|
||||
fp32 joint_horizontal_torque_temp1_R, joint_horizontal_torque_temp2_R;
|
||||
fp32 joint_horizontal_torque_temp1_L, joint_horizontal_torque_temp2_L;
|
||||
|
||||
fp32 yaw_torque;
|
||||
|
||||
// -------- vertical force --------
|
||||
fp32 joint_roll_torque_L, joint_roll_torque_R;
|
||||
fp32 joint_stand_torque_L, joint_stand_torque_R;
|
||||
|
||||
fp32 joint_vertical_torque_L, joint_vertical_torque_R;
|
||||
fp32 joint_real_vertical_torque_L, joint_real_vertical_torque_R;
|
||||
|
||||
fp32 joint_vertical_torque_temp1_R, joint_vertical_torque_temp2_R;
|
||||
fp32 joint_vertical_torque_temp1_L, joint_vertical_torque_temp2_L;
|
||||
|
||||
} torque_info_t;
|
||||
|
||||
typedef struct
|
||||
{
|
||||
fp32 J1_L,J2_L;
|
||||
fp32 J3_L,J4_L;
|
||||
fp32 J1_R,J2_R;
|
||||
fp32 J3_R,J4_R;
|
||||
fp32 invJ1_L,invJ2_L;
|
||||
fp32 invJ3_L,invJ4_L;
|
||||
fp32 invJ1_R,invJ2_R;
|
||||
fp32 invJ3_R,invJ4_R;
|
||||
|
||||
} mapping_info_t;
|
||||
|
||||
typedef struct
|
||||
{
|
||||
const HT_motor_measure_t *motor_measure;
|
||||
chassis_motor_mode_e motor_mode, last_motor_mode;
|
||||
|
||||
bool_t offline_flag;
|
||||
|
||||
fp32 position;
|
||||
fp32 init_position;
|
||||
fp32 position_offset;
|
||||
|
||||
fp32 velocity;
|
||||
fp32 torque_out, torque_get;
|
||||
fp32 max_torque, min_torque;
|
||||
} joint_motor_t;
|
||||
|
||||
typedef struct
|
||||
{
|
||||
motor_measure_t motor_measure;
|
||||
chassis_motor_mode_e motor_mode, last_motor_mode;
|
||||
|
||||
bool_t offline_flag;
|
||||
|
||||
fp32 distance, distance_offset, last_position, position, turns;
|
||||
fp32 speed;
|
||||
fp32 torque_out, torque_get;
|
||||
|
||||
} foot_motor_t;
|
||||
|
||||
typedef struct
|
||||
{
|
||||
bool_t init_flag;
|
||||
suspend_flag_e suspend_flag_L, last_suspend_flag_L;
|
||||
suspend_flag_e suspend_flag_R, last_suspend_flag_R;
|
||||
bool_t Ignore_Off_Ground;
|
||||
bool_t abnormal_flag;
|
||||
bool_t static_flag, last_static_flag;
|
||||
bool_t moving_flag, last_moving_flag;
|
||||
bool_t rotation_flag;
|
||||
bool_t controlling_flag;
|
||||
bool_t set_pos_after_moving;
|
||||
bool_t overpower_warning_flag;
|
||||
bool_t last_overpower_warning_flag;
|
||||
bool_t stablize_high_flag;
|
||||
bool_t last_stablize_high_flag;
|
||||
} flag_info_t;
|
||||
|
||||
typedef struct
|
||||
{
|
||||
pid_type_def buffer_control_pid;
|
||||
pid_type_def cap_pid;
|
||||
pid_type_def scale_down_pid;
|
||||
} pid_info_t;
|
||||
|
||||
typedef struct
|
||||
{
|
||||
mode_t mode;
|
||||
chassis_posture_info_t chassis_posture_info;
|
||||
torque_info_t torque_info;
|
||||
mapping_info_t mapping_info;
|
||||
flag_info_t flag_info;
|
||||
pid_info_t pid_info;
|
||||
const Gimbal_ctrl_t *chassis_rc_ctrl;
|
||||
|
||||
joint_motor_t joint_motor_1, joint_motor_2, joint_motor_3, joint_motor_4;
|
||||
foot_motor_t foot_motor_L, foot_motor_R;
|
||||
|
||||
}chassis_control_t;
|
||||
|
||||
enum Chassis_Mode
|
||||
{
|
||||
Chassis_No_Force = 0,
|
||||
Follow_Gimbal,
|
||||
Follow_Gimbal_90Deg,
|
||||
No_Follow,
|
||||
Rotate,
|
||||
// TK_MODE,
|
||||
};
|
||||
|
||||
extern enum Chassis_Mode chassis_mode;
|
||||
extern chassis_control_t chassis_control;
|
||||
extern fp32 roll_PD[2];
|
||||
|
||||
#endif
|
||||
@ -27,6 +27,7 @@
|
||||
/* Private includes ----------------------------------------------------------*/
|
||||
/* USER CODE BEGIN Includes */
|
||||
|
||||
#include "task/user_task.h"
|
||||
/* USER CODE END Includes */
|
||||
|
||||
/* Private typedef -----------------------------------------------------------*/
|
||||
@ -125,7 +126,8 @@ void MX_FREERTOS_Init(void) {
|
||||
|
||||
/* USER CODE BEGIN RTOS_THREADS */
|
||||
/* add threads, ... */
|
||||
/* USER CODE END RTOS_THREADS */
|
||||
osThreadNew(Task_Init, NULL, &attr_init); // 创建初始化任务
|
||||
/* USER CODE END RTOS_THREADS */
|
||||
|
||||
/* USER CODE BEGIN RTOS_EVENTS */
|
||||
/* add events, ... */
|
||||
@ -145,12 +147,8 @@ void StartDefaultTask(void *argument)
|
||||
/* init code for USB_DEVICE */
|
||||
MX_USB_DEVICE_Init();
|
||||
/* USER CODE BEGIN StartDefaultTask */
|
||||
/* Infinite loop */
|
||||
for(;;)
|
||||
{
|
||||
osDelay(1);
|
||||
}
|
||||
/* USER CODE END StartDefaultTask */
|
||||
osThreadTerminate(osThreadGetId());
|
||||
/* USER CODE END StartDefaultTask */
|
||||
}
|
||||
|
||||
/* Private application code --------------------------------------------------*/
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
321
HerKules_VOCAL_SJ_LQR_v4_with_data.m
Normal file
321
HerKules_VOCAL_SJ_LQR_v4_with_data.m
Normal file
@ -0,0 +1,321 @@
|
||||
% v1：这份LQR程序是参考我之前写的哈工程LQR程序以及小周写的AB矩阵求解器优化后写出来的，感谢周神（2024/05/07）
|
||||
% v2：添加了可以专门调试定腿长的功能（2024/05/08）
|
||||
% v3：优化部分注释，添加单位说明（2024/05/15）
|
||||
% v4: 优化了输出，输出矩阵K的系数可以真正的复制到C里（2024/05/16）
|
||||
|
||||
% 以下所有变量含义参考2023上交轮腿电控开源（https://bbs.robomaster.com/forum.php?mod=viewthread&tid=22756）所使用符号含义
|
||||
|
||||
%%%%%%%%%%%%%%%%%%%%%%%%%Step 0：重置程序，定义变量%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
tic
|
||||
clear all
|
||||
clc
|
||||
|
||||
% 定义机器人机体参数
|
||||
syms R_w % 驱动轮半径
|
||||
syms R_l % 驱动轮轮距/2
|
||||
syms l_l l_r % 左右腿长
|
||||
syms l_wl l_wr % 驱动轮质心到左右腿部质心距离
|
||||
syms l_bl l_br % 机体质心到左右腿部质心距离
|
||||
syms l_c % 机体质心到腿部关节中心点距离
|
||||
syms m_w m_l m_b % 驱动轮质量 腿部质量 机体质量
|
||||
syms I_w % 驱动轮转动惯量 (自然坐标系法向)
|
||||
syms I_ll I_lr % 驱动轮左右腿部转动惯量 (自然坐标系法向，实际上会变化)
|
||||
syms I_b % 机体转动惯量 (自然坐标系法向)
|
||||
syms I_z % 机器人z轴转动惯量 (简化为常量)
|
||||
|
||||
% 定义其他独立变量并补充其导数
|
||||
syms theta_wl theta_wr % 左右驱动轮转角
|
||||
syms dtheta_wl dtheta_wr
|
||||
syms ddtheta_wl ddtheta_wr ddtheta_ll ddtheta_lr ddtheta_b
|
||||
|
||||
% 定义状态向量
|
||||
syms s ds phi dphi theta_ll dtheta_ll theta_lr dtheta_lr theta_b dtheta_b
|
||||
|
||||
% 定义控制向量
|
||||
syms T_wl T_wr T_bl T_br
|
||||
|
||||
% 输入物理参数：重力加速度
|
||||
syms g
|
||||
|
||||
|
||||
|
||||
%%%%%%%%%%%%%%%%%%%%%%%Step 1：解方程，求控制矩阵A，B%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
% 通过原文方程组(3.11)-(3.15)，求出ddtheta_wl,ddtheta_wr,ddtheta_ll,ddtheta_lr,ddtheta_b表达式
|
||||
eqn1 = (I_w*l_l/R_w+m_w*R_w*l_l+m_l*R_w*l_bl)*ddtheta_wl+(m_l*l_wl*l_bl-I_ll)*ddtheta_ll+(m_l*l_wl+m_b*l_l/2)*g*theta_ll+T_bl-T_wl*(1+l_l/R_w)==0;
|
||||
eqn2 = (I_w*l_r/R_w+m_w*R_w*l_r+m_l*R_w*l_br)*ddtheta_wr+(m_l*l_wr*l_br-I_lr)*ddtheta_lr+(m_l*l_wr+m_b*l_r/2)*g*theta_lr+T_br-T_wr*(1+l_r/R_w)==0;
|
||||
eqn3 = -(m_w*R_w*R_w+I_w+m_l*R_w*R_w+m_b*R_w*R_w/2)*ddtheta_wl-(m_w*R_w*R_w+I_w+m_l*R_w*R_w+m_b*R_w*R_w/2)*ddtheta_wr-(m_l*R_w*l_wl+m_b*R_w*l_l/2)*ddtheta_ll-(m_l*R_w*l_wr+m_b*R_w*l_r/2)*ddtheta_lr+T_wl+T_wr==0;
|
||||
eqn4 = (m_w*R_w*l_c+I_w*l_c/R_w+m_l*R_w*l_c)*ddtheta_wl+(m_w*R_w*l_c+I_w*l_c/R_w+m_l*R_w*l_c)*ddtheta_wr+m_l*l_wl*l_c*ddtheta_ll+m_l*l_wr*l_c*ddtheta_lr-I_b*ddtheta_b+m_b*g*l_c*theta_b-(T_wl+T_wr)*l_c/R_w-(T_bl+T_br)==0;
|
||||
eqn5 = ((I_z*R_w)/(2*R_l)+I_w*R_l/R_w)*ddtheta_wl-((I_z*R_w)/(2*R_l)+I_w*R_l/R_w)*ddtheta_wr+(I_z*l_l)/(2*R_l)*ddtheta_ll-(I_z*l_r)/(2*R_l)*ddtheta_lr-T_wl*R_l/R_w+T_wr*R_l/R_w==0;
|
||||
[ddtheta_wl,ddtheta_wr,ddtheta_ll,ddtheta_lr,ddtheta_b] = solve(eqn1,eqn2,eqn3,eqn4,eqn5,ddtheta_wl,ddtheta_wr,ddtheta_ll,ddtheta_lr,ddtheta_b);
|
||||
|
||||
|
||||
% 通过计算雅可比矩阵的方法得出控制矩阵A，B所需要的全部偏导数
|
||||
J_A = jacobian([ddtheta_wl,ddtheta_wr,ddtheta_ll,ddtheta_lr,ddtheta_b],[theta_ll,theta_lr,theta_b]);
|
||||
J_B = jacobian([ddtheta_wl,ddtheta_wr,ddtheta_ll,ddtheta_lr,ddtheta_b],[T_wl,T_wr,T_bl,T_br]);
|
||||
|
||||
% 定义矩阵A，B，将指定位置的数值根据上述偏导数计算出来并填入
|
||||
A = sym('A',[10 10]);
|
||||
B = sym('B',[10 4]);
|
||||
|
||||
% 填入A数据：a25,a27,a29,a45,a47,a49,a65,a67,a69,a85,a87,a89,a105,a107,a109
|
||||
for p = 5:2:9
|
||||
A_index = (p - 3)/2;
|
||||
A(2,p) = R_w*(J_A(1,A_index) + J_A(2,A_index))/2;
|
||||
A(4,p) = (R_w*(- J_A(1,A_index) + J_A(2,A_index)))/(2*R_l) - (l_l*J_A(3,A_index))/(2*R_l) + (l_r*J_A(4,A_index))/(2*R_l);
|
||||
for q = 6:2:10
|
||||
A(q,p) = J_A(q/2,A_index);
|
||||
end
|
||||
end
|
||||
|
||||
% A的以下数值为1：a12,a34,a56,a78,a910，其余数值为0
|
||||
for r = 1:10
|
||||
if rem(r,2) == 0
|
||||
A(r,1) = 0; A(r,2) = 0; A(r,3) = 0; A(r,4) = 0; A(r,6) = 0; A(r,8) = 0; A(r,10) = 0;
|
||||
else
|
||||
A(r,:) = zeros(1,10);
|
||||
A(r,r+1) = 1;
|
||||
end
|
||||
end
|
||||
|
||||
% 填入B数据：b21,b22,b23,b24,b41,b42,b43,b44,b61,b62,b63,b64,b81,b82,b83,b84,b101,b102,b103,b104,
|
||||
for h = 1:4
|
||||
B(2,h) = R_w*(J_B(1,h) + J_B(2,h))/2;
|
||||
B(4,h) = (R_w*(- J_B(1,h) + J_B(2,h)))/(2*R_l) - (l_l*J_B(3,h))/(2*R_l) + (l_r*J_B(4,h))/(2*R_l);
|
||||
for f = 6:2:10
|
||||
B(f,h) = J_B(f/2,h);
|
||||
end
|
||||
end
|
||||
|
||||
% B的其余数值为0
|
||||
for e = 1:2:9
|
||||
B(e,:) = zeros(1,4);
|
||||
end
|
||||
|
||||
|
||||
|
||||
%%%%%%%%%%%%%%%%%%%%%Step 2：输入参数（可以修改的部分）%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
% 物理参数赋值（唯一此处不可改变！），后面的数据通过增加后缀_ac区分模型符号和实际数据
|
||||
g_ac = 9.81;
|
||||
|
||||
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
% 此处可以输入机器人机体基本参数 %
|
||||
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
%%%%%%%%%%%%%%%%%%%%%%%%%%%机器人机体与轮部参数%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
R_w_ac = 0.9; % 驱动轮半径 （单位：m）
|
||||
R_l_ac = 0.25; % 两个驱动轮之间距离/2 （单位：m）
|
||||
l_c_ac = 0.037; % 机体质心到腿部关节中心点距离 （单位：m）
|
||||
m_w_ac = 0.8; m_l_ac = 1.6183599; m_b_ac = 11.542; % 驱动轮质量 腿部质量 机体质量 （单位：kg）
|
||||
I_w_ac = (3510000)*10^(-7); % 驱动轮转动惯量 （单位：kg m^2）
|
||||
I_b_ac = 0.260; % 机体转动惯量(自然坐标系法向) （单位：kg m^2）
|
||||
I_z_ac = 0.226; % 机器人z轴转动惯量 （单位：kg m^2）
|
||||
|
||||
%%%%%%%%%%%%%%%%%%%%%%机器人腿部参数（定腿长调试用）%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
% 如果需要使用此部分，请去掉120-127行以及215-218行注释，然后将224行之后的所有代码注释掉
|
||||
% 或者点击左侧数字"224"让程序只运行行之前的内容并停止
|
||||
|
||||
l_l_ac = 0.18; % 左腿摆杆长度 （左腿对应数据） （单位：m）
|
||||
l_wl_ac = 0.05; % 左驱动轮质心到左腿摆杆质心距离 （单位：m）
|
||||
l_bl_ac = 0.13; % 机体转轴到左腿摆杆质心距离 （单位：m）
|
||||
I_ll_ac = 0.02054500; % 左腿摆杆转动惯量 （单位：kg m^2）
|
||||
l_r_ac = 0.18; % 右腿摆杆长度 （右腿对应数据） （单位：m）
|
||||
l_wr_ac = 0.05; % 右驱动轮质心到右腿摆杆质心距离 （单位：m）
|
||||
l_br_ac = 0.13; % 机体转轴到右腿摆杆质心距离 （单位：m）
|
||||
I_lr_ac = 0.02054500; % 右腿摆杆转动惯量 （单位：kg m^2）
|
||||
|
||||
% 机体转轴定义参考哈工程开源（https://zhuanlan.zhihu.com/p/563048952），是左右
|
||||
% 两侧两个关节电机之间的中间点相连所形成的轴
|
||||
% （如果目的是小板凳，考虑使左右腿相关数据一致）
|
||||
|
||||
%%%%%%%%%%%%%%%%%%%%%%%%%%%机器人腿部参数数据集%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
% 根据不同腿长长度，先针对左腿测量出对应的l_wl，l_bl，和I_ll
|
||||
% 通过以下方式记录数据: 矩阵分4列，
|
||||
% 第一列为左腿腿长范围区间中所有小数点精度0.01的长度，例如：0.09，0.18，单位：m
|
||||
% 第二列为l_wl，单位：m
|
||||
% 第三列为l_bl，单位：m
|
||||
% 第四列为I_ll，单位：kg m^2
|
||||
% （注意单位别搞错！）
|
||||
% 行数根据L_0范围区间（，单位cm时）的整数数量进行调整
|
||||
|
||||
Leg_data_l = [0.11, 0.0480, 0.0620, 0.01819599;
|
||||
0.12, 0.0470, 0.0730, 0.01862845;
|
||||
0.13, 0.0476, 0.0824, 0.01898641;
|
||||
0.14, 0.0480, 0.0920, 0.01931342;
|
||||
0.15, 0.0490, 0.1010, 0.01962521;
|
||||
0.16, 0.0500, 0.1100, 0.01993092;
|
||||
0.17, 0.0510, 0.1190, 0.02023626;
|
||||
0.18, 0.0525, 0.1275, 0.02054500;
|
||||
0.19, 0.0539, 0.1361, 0.02085969;
|
||||
0.20, 0.0554, 0.1446, 0.02118212;
|
||||
0.21, 0.0570, 0.1530, 0.02151357;
|
||||
0.22, 0.0586, 0.1614, 0.02185496;
|
||||
0.23, 0.0600, 0.1700, 0.02220695;
|
||||
0.24, 0.0621, 0.1779, 0.02256999;
|
||||
0.25, 0.0639, 0.1861, 0.02294442;
|
||||
0.26, 0.0657, 0.1943, 0.02333041;
|
||||
0.27, 0.0676, 0.2024, 0.02372806;
|
||||
0.28, 0.0700, 0.2100, 0.02413735;
|
||||
0.29, 0.0713, 0.2187, 0.02455817;
|
||||
0.30, 0.0733, 0.2267, 0.02499030];
|
||||
% 以上数据应通过实际测量或sw图纸获得
|
||||
|
||||
% 由于左右腿部数据通常完全相同，我们通过复制的方式直接定义右腿的全部数据集
|
||||
% 矩阵分4列，第一列为右腿腿长范围区间中（，单位cm时）的整数腿长l_r*0.01，第二列为l_wr，第三列为l_br，第四列为I_lr）
|
||||
Leg_data_r = Leg_data_l;
|
||||
|
||||
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
% 此处可以输入QR矩阵 %
|
||||
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
% 矩阵Q中，以下列分别对应：
|
||||
% s ds phi dphi theta_ll dtheta_ll theta_lr dtheta_lr theta_b dtheta_b
|
||||
lqr_Q = [1, 0, 0, 0, 0, 0, 0, 0, 0, 0;
|
||||
0, 2, 0, 0, 0, 0, 0, 0, 0, 0;
|
||||
0, 0, 12000, 0, 0, 0, 0, 0, 0, 0;
|
||||
0, 0, 0, 200, 0, 0, 0, 0, 0, 0;
|
||||
0, 0, 0, 0, 1000, 0, 0, 0, 0, 0;
|
||||
0, 0, 0, 0, 0, 1, 0, 0, 0, 0;
|
||||
0, 0, 0, 0, 0, 0, 1000, 0, 0, 0;
|
||||
0, 0, 0, 0, 0, 0, 0, 1, 0, 0;
|
||||
0, 0, 0, 0, 0, 0, 0, 0, 20000, 0;
|
||||
0, 0, 0, 0, 0, 0, 0, 0, 0, 1];
|
||||
% 其中：
|
||||
% s : 自然坐标系下机器人水平方向移动距离，单位：m，ds为其导数
|
||||
% phi ：机器人水平方向移动时yaw偏航角度，dphi为其导数
|
||||
% theta_ll：左腿摆杆与竖直方向（自然坐标系z轴）夹角，dtheta_ll为其导数
|
||||
% theta_lr：右腿摆杆与竖直方向（自然坐标系z轴）夹角，dtheta_lr为其导数
|
||||
% theta_b ：机体与自然坐标系水平夹角，dtheta_b为其导数
|
||||
|
||||
% 矩阵中，以下列分别对应：
|
||||
% T_wl T_wr T_bl T_br
|
||||
lqr_R = [0.25, 0, 0, 0;
|
||||
0, 0.25, 0, 0;
|
||||
0, 0, 1.5, 0;
|
||||
0, 0, 0, 1.5];
|
||||
% 其中：
|
||||
% T_wl: 左侧驱动轮输出力矩
|
||||
% T_wr：右侧驱动轮输出力矩
|
||||
% T_bl：左侧髋关节输出力矩
|
||||
% T_br：右腿髋关节输出力矩
|
||||
% 单位皆为Nm
|
||||
|
||||
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
|
||||
%%%%%%%%%%%%%%%%%%%%%Step 2.5：求解矩阵（定腿长调试用）%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
% 如果需要使用此部分，请去掉120-127行以及215-218行注释，然后将224行之后的所有代码注释掉，
|
||||
% 或者点击左侧数字"224"让程序只运行行之前的内容并停止
|
||||
K = get_K_from_LQR(R_w,R_l,l_l,l_r,l_wl,l_wr,l_bl,l_br,l_c,m_w,m_l,m_b,I_w,I_ll,I_lr,I_b,I_z,g, ...
|
||||
R_w_ac,R_l_ac,l_l_ac,l_r_ac,l_wl_ac,l_wr_ac,l_bl_ac,l_br_ac, ...
|
||||
l_c_ac,m_w_ac,m_l_ac,m_b_ac,I_w_ac,I_ll_ac,I_lr_ac,I_b_ac,I_z_ac,g_ac, ...
|
||||
A,B,lqr_Q,lqr_R)
|
||||
K = sprintf([strjoin(repmat({'%.5g'},1,size(K,2)),', ') '\n'], K.')
|
||||
|
||||
|
||||
%%%%%%%%%%%%%%%%%%%%%%%%%%Step 3：拟合控制律函数%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
sample_size = size(Leg_data_l,1)^2; % 单个K_ij拟合所需要的样本数
|
||||
|
||||
length = size(Leg_data_l,1); % 测量腿部数据集的行数
|
||||
|
||||
% 定义所有K_ij依据l_l,l_r变化的表格，每一个表格有3列，第一列是l_l，第二列
|
||||
% 是l_r，第三列是对应的K_ij的数值
|
||||
K_sample = zeros(sample_size,3,40); % 40是因为增益矩阵K应该是4行10列。
|
||||
|
||||
for i = 1:length
|
||||
for j = 1:length
|
||||
index = (i - 1)*length + j;
|
||||
l_l_ac = Leg_data_l(i,1); % 提取左腿对应的数据
|
||||
l_wl_ac = Leg_data_l(i,2);
|
||||
l_bl_ac = Leg_data_l(i,3);
|
||||
I_ll_ac = Leg_data_l(i,4);
|
||||
l_r_ac = Leg_data_r(j,1); % 提取右腿对应的数据
|
||||
l_wr_ac = Leg_data_r(j,2);
|
||||
l_br_ac = Leg_data_r(j,3);
|
||||
I_lr_ac = Leg_data_r(j,4);
|
||||
for k = 1:40
|
||||
K_sample(index,1,k) = l_l_ac;
|
||||
K_sample(index,2,k) = l_r_ac;
|
||||
end
|
||||
K = get_K_from_LQR(R_w,R_l,l_l,l_r,l_wl,l_wr,l_bl,l_br,l_c,m_w,m_l,m_b,I_w,I_ll,I_lr,I_b,I_z,g, ...
|
||||
R_w_ac,R_l_ac,l_l_ac,l_r_ac,l_wl_ac,l_wr_ac,l_bl_ac,l_br_ac, ...
|
||||
l_c_ac,m_w_ac,m_l_ac,m_b_ac,I_w_ac,I_ll_ac,I_lr_ac,I_b_ac,I_z_ac,g_ac, ...
|
||||
A,B,lqr_Q,lqr_R);
|
||||
% 根据指定的l_l,l_r输入对应的K_ij的数值
|
||||
for l = 1:4
|
||||
for m = 1:10
|
||||
K_sample(index,3,(l - 1)*10 + m) = K(l,m);
|
||||
end
|
||||
end
|
||||
end
|
||||
end
|
||||
|
||||
% 创建收集全部K_ij的多项式拟合的全部系数的集合
|
||||
K_Fit_Coefficients = zeros(40,6);
|
||||
for n = 1:40
|
||||
K_Surface_Fit = fit([K_sample(:,1,n),K_sample(:,2,n)],K_sample(:,3,n),'poly22');
|
||||
K_Fit_Coefficients(n,:) = coeffvalues(K_Surface_Fit); % 拟合并提取出拟合的系数结果
|
||||
end
|
||||
Polynomial_expression = formula(K_Surface_Fit)
|
||||
|
||||
% 最终返回的结果K_Fit_Coefficients是一个40行6列矩阵，每一行分别对应一个K_ij的多项式拟合的全部系数
|
||||
% 每一行和K_ij的对应关系如下：
|
||||
% - 第1行对应K_1,1
|
||||
% - 第14行对应K_2,4
|
||||
% - 第22行对应K_3,2
|
||||
% - 第37行对应K_4,7
|
||||
% ... 其他行对应关系类似
|
||||
% 拟合出的函数表达式为 p(x,y) = p00 + p10*x + p01*y + p20*x^2 + p11*x*y + p02*y^2
|
||||
% 其中x对应左腿腿长l_l，y对应右腿腿长l_r
|
||||
% K_Fit_Coefficients每一列分别对应全部K_ij的多项式拟合的单个系数
|
||||
% 每一列和系数pij的对应关系如下：
|
||||
% - 第1列对应p00
|
||||
% - 第2列对应p10
|
||||
% - 第3列对应p01
|
||||
% - 第4列对应p20
|
||||
% - 第5列对应p11
|
||||
% - 第6列对应p02
|
||||
K_Fit_Coefficients = sprintf([strjoin(repmat({'%.5g'},1,size(K_Fit_Coefficients,2)),', ') '\n'], K_Fit_Coefficients.')
|
||||
|
||||
% 正确食用方法：
|
||||
% 1.在C代码中写出控制律K矩阵的全部多项式，其中每一个多项式的表达式为：
|
||||
% p(l_l,l_r) = p00 + p10*l_l + p01*l_r + p20*l_l^2 + p11*l_l*l_r + p02*l_r^2
|
||||
% 2.并填入对应系数即可
|
||||
|
||||
toc
|
||||
|
||||
%%%%%%%%%%%%%%%%%%%%%%%%%以下信息仅供参考，可忽略%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
% 如有需要可以把所有K_ij画出图来参考，可以去掉以下注释
|
||||
% 此版本只能同时查看其中一个K_ij，同时查看多个的功能下次更新
|
||||
% （前面的蛆，以后再来探索吧（bushi
|
||||
|
||||
|
||||
|
||||
%%%%%%%%%%%%%%%%%%%%%%%%%%得出控制矩阵K使用的函数%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
function K = get_K_from_LQR(R_w,R_l,l_l,l_r,l_wl,l_wr,l_bl,l_br,l_c,m_w,m_l,m_b,I_w,I_ll,I_lr,I_b,I_z,g, ...
|
||||
R_w_ac,R_l_ac,l_l_ac,l_r_ac,l_wl_ac,l_wr_ac,l_bl_ac,l_br_ac, ...
|
||||
l_c_ac,m_w_ac,m_l_ac,m_b_ac,I_w_ac,I_ll_ac,I_lr_ac,I_b_ac,I_z_ac,g_ac, ...
|
||||
A,B,lqr_Q,lqr_R)
|
||||
% 基于机体以及物理参数，获得控制矩阵A，B的全部数值
|
||||
A_ac = subs(A,[R_w R_l l_l l_r l_wl l_wr l_bl l_br l_c m_w m_l m_b I_w I_ll I_lr I_b I_z g], ...
|
||||
[R_w_ac R_l_ac l_l_ac l_r_ac l_wl_ac l_wr_ac l_bl_ac l_br_ac l_c_ac ...
|
||||
m_w_ac m_l_ac m_b_ac I_w_ac I_ll_ac I_lr_ac I_b_ac I_z_ac g_ac]);
|
||||
B_ac = subs(B,[R_w R_l l_l l_r l_wl l_wr l_bl l_br l_c m_w m_l m_b I_w I_ll I_lr I_b I_z g], ...
|
||||
[R_w_ac R_l_ac l_l_ac l_r_ac l_wl_ac l_wr_ac l_bl_ac l_br_ac l_c_ac ...
|
||||
m_w_ac m_l_ac m_b_ac I_w_ac I_ll_ac I_lr_ac I_b_ac I_z_ac g_ac]);
|
||||
|
||||
% 根据以上信息和提供的矩阵Q和R求解Riccati方程，获得增益矩阵K
|
||||
% P为Riccati方程的解，矩阵L可以无视
|
||||
[P,K,L_k] = icare(A_ac,B_ac,lqr_Q,lqr_R,[],[],[]);
|
||||
end
|
||||
|
||||
67
LQR_修正建议.md
Normal file
67
LQR_修正建议.md
Normal file
@ -0,0 +1,67 @@
|
||||
## 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模型进行修正。
|
||||
主要是要确保状态向量定义、增益矩阵维度和控制律计算都与理论模型一致。
|
||||
@ -11,6 +11,7 @@
|
||||
<ToolsetNumber>0x4</ToolsetNumber>
|
||||
<ToolsetName>ARM-ADS</ToolsetName>
|
||||
<pArmCC>6160000::V6.16::ARMCLANG</pArmCC>
|
||||
<pCCUsed>6160000::V6.16::ARMCLANG</pCCUsed>
|
||||
<uAC6>1</uAC6>
|
||||
<TargetOption>
|
||||
<TargetCommonOption>
|
||||
@ -81,7 +82,7 @@
|
||||
</BeforeMake>
|
||||
<AfterMake>
|
||||
<RunUserProg1>0</RunUserProg1>
|
||||
<RunUserProg2>1</RunUserProg2>
|
||||
<RunUserProg2>0</RunUserProg2>
|
||||
<UserProg1Name></UserProg1Name>
|
||||
<UserProg2Name></UserProg2Name>
|
||||
<UserProg1Dos16Mode>0</UserProg1Dos16Mode>
|
||||
@ -313,7 +314,7 @@
|
||||
</ArmAdsMisc>
|
||||
<Cads>
|
||||
<interw>1</interw>
|
||||
<Optim>4</Optim>
|
||||
<Optim>2</Optim>
|
||||
<oTime>0</oTime>
|
||||
<SplitLS>0</SplitLS>
|
||||
<OneElfS>1</OneElfS>
|
||||
@ -339,7 +340,7 @@
|
||||
<MiscControls></MiscControls>
|
||||
<Define>USE_HAL_DRIVER,STM32F407xx</Define>
|
||||
<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</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;..\User</IncludePath>
|
||||
</VariousControls>
|
||||
</Cads>
|
||||
<Aads>
|
||||
@ -730,6 +731,191 @@
|
||||
</File>
|
||||
</Files>
|
||||
</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>
|
||||
<GroupName>::CMSIS</GroupName>
|
||||
</Group>
|
||||
@ -750,4 +936,13 @@
|
||||
<files/>
|
||||
</RTE>
|
||||
|
||||
<LayerInfo>
|
||||
<Layers>
|
||||
<Layer>
|
||||
<LayName>DevC</LayName>
|
||||
<LayPrjMark>1</LayPrjMark>
|
||||
</Layer>
|
||||
</Layers>
|
||||
</LayerInfo>
|
||||
|
||||
</Project>
|
||||
|
||||
@ -61,6 +61,37 @@
|
||||
"devc\usbd_ctlreq.o"
|
||||
"devc\usbd_ioreq.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"
|
||||
--summary_stderr --info summarysizes --map --load_addr_map_info --xref --callgraph --symbols
|
||||
--info sizes --info totals --info unused --info veneers
|
||||
|
||||
2
MDK-ARM/DevC/ExtDll.iex
Normal file
2
MDK-ARM/DevC/ExtDll.iex
Normal file
@ -0,0 +1,2 @@
|
||||
[EXTDLL]
|
||||
Count=0
|
||||
9
MDK-ARM/EventRecorderStub.scvd
Normal file
9
MDK-ARM/EventRecorderStub.scvd
Normal file
@ -0,0 +1,9 @@
|
||||
<?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
Normal file
297
User/README.md
Normal file
@ -0,0 +1,297 @@
|
||||
# 轮腿机器人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
Normal file
16
User/bsp/bsp.h
Normal file
@ -0,0 +1,16 @@
|
||||
#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
Normal file
132
User/bsp/bsp_config.yaml
Normal file
@ -0,0 +1,132 @@
|
||||
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
Normal file
701
User/bsp/can.c
Normal file
@ -0,0 +1,701 @@
|
||||
/* 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
Normal file
233
User/bsp/can.h
Normal file
@ -0,0 +1,233 @@
|
||||
#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
Normal file
122
User/bsp/dwt.c
Normal file
@ -0,0 +1,122 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
* @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
Normal file
37
User/bsp/dwt.h
Normal file
@ -0,0 +1,37 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
* @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
Normal file
114
User/bsp/gpio.c
Normal file
@ -0,0 +1,114 @@
|
||||
/* 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
Normal file
45
User/bsp/gpio.h
Normal file
@ -0,0 +1,45 @@
|
||||
#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
Normal file
14
User/bsp/mm.c
Normal file
@ -0,0 +1,14 @@
|
||||
/* 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
Normal file
20
User/bsp/mm.h
Normal file
@ -0,0 +1,20 @@
|
||||
#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
Normal file
109
User/bsp/pwm.c
Normal file
@ -0,0 +1,109 @@
|
||||
/* 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
Normal file
47
User/bsp/pwm.h
Normal file
@ -0,0 +1,47 @@
|
||||
#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
Normal file
165
User/bsp/spi.c
Normal file
@ -0,0 +1,165 @@
|
||||
/* 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, ®, 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, ®, 1u, true);
|
||||
return BSP_SPI_Transmit(spi, data, size, true);
|
||||
}
|
||||
58
User/bsp/spi.h
Normal file
58
User/bsp/spi.h
Normal file
@ -0,0 +1,58 @@
|
||||
#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
Normal file
65
User/bsp/time.c
Normal file
@ -0,0 +1,65 @@
|
||||
/* 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
Normal file
31
User/bsp/time.h
Normal file
@ -0,0 +1,31 @@
|
||||
#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
Normal file
139
User/bsp/uart.c
Normal file
@ -0,0 +1,139 @@
|
||||
/* 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
Normal file
53
User/bsp/uart.h
Normal file
@ -0,0 +1,53 @@
|
||||
#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
Normal file
405
User/component/ahrs.c
Normal file
@ -0,0 +1,405 @@
|
||||
/*
|
||||
开源的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
Normal file
98
User/component/ahrs.h
Normal file
@ -0,0 +1,98 @@
|
||||
/*
|
||||
开源的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
Normal file
375
User/component/cmd.c
Normal file
@ -0,0 +1,375 @@
|
||||
/*
|
||||
控制命令
|
||||
*/
|
||||
|
||||
#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
Normal file
306
User/component/cmd.h
Normal file
@ -0,0 +1,306 @@
|
||||
/*
|
||||
控制命令
|
||||
*/
|
||||
|
||||
#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
Normal file
21
User/component/component_config.yaml
Normal file
@ -0,0 +1,21 @@
|
||||
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
Normal file
185
User/component/filter.c
Normal file
@ -0,0 +1,185 @@
|
||||
/*
|
||||
各类滤波器。
|
||||
*/
|
||||
|
||||
#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
Normal file
104
User/component/filter.h
Normal file
@ -0,0 +1,104 @@
|
||||
/*
|
||||
各类滤波器。
|
||||
*/
|
||||
|
||||
#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
Normal file
113
User/component/kinematics.c
Normal file
@ -0,0 +1,113 @@
|
||||
/*
|
||||
一些运动学相关的计算函数
|
||||
*/
|
||||
|
||||
#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
Normal file
43
User/component/kinematics.h
Normal file
@ -0,0 +1,43 @@
|
||||
#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
Normal file
107
User/component/limiter.c
Normal file
@ -0,0 +1,107 @@
|
||||
/*
|
||||
限制器
|
||||
*/
|
||||
|
||||
#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
Normal file
55
User/component/limiter.h
Normal file
@ -0,0 +1,55 @@
|
||||
/*
|
||||
限制器
|
||||
*/
|
||||
|
||||
#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
Normal file
229
User/component/lqr.c
Normal file
@ -0,0 +1,229 @@
|
||||
/*
|
||||
* 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
Normal file
168
User/component/lqr.h
Normal file
@ -0,0 +1,168 @@
|
||||
#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
Normal file
158
User/component/pid.c
Normal file
@ -0,0 +1,158 @@
|
||||
/*
|
||||
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
Normal file
95
User/component/pid.h
Normal file
@ -0,0 +1,95 @@
|
||||
/*
|
||||
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
Normal file
132
User/component/user_math.c
Normal file
@ -0,0 +1,132 @@
|
||||
/*
|
||||
自定义的数学运算。
|
||||
*/
|
||||
|
||||
#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
Normal file
161
User/component/user_math.h
Normal file
@ -0,0 +1,161 @@
|
||||
/*
|
||||
自定义的数学运算。
|
||||
*/
|
||||
|
||||
#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
Normal file
381
User/component/vmc.c
Normal file
@ -0,0 +1,381 @@
|
||||
/*
|
||||
* 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
Normal file
196
User/component/vmc.h
Normal file
@ -0,0 +1,196 @@
|
||||
#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
Normal file
44
User/device/buzzer.c
Normal file
@ -0,0 +1,44 @@
|
||||
#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
Normal file
35
User/device/buzzer.h
Normal file
@ -0,0 +1,35 @@
|
||||
#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
Normal file
31
User/device/device.h
Normal file
@ -0,0 +1,31 @@
|
||||
#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
Normal file
19
User/device/device_config.yaml
Normal file
@ -0,0 +1,19 @@
|
||||
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
Normal file
271
User/device/dm_imu.c
Normal file
@ -0,0 +1,271 @@
|
||||
/*
|
||||
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
Normal file
90
User/device/dm_imu.h
Normal file
@ -0,0 +1,90 @@
|
||||
#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
Normal file
85
User/device/dr16.c
Normal file
@ -0,0 +1,85 @@
|
||||
/*
|
||||
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
Normal file
47
User/device/dr16.h
Normal file
@ -0,0 +1,47 @@
|
||||
#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
Normal file
39
User/device/motor.c
Normal file
@ -0,0 +1,39 @@
|
||||
/*
|
||||
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
Normal file
52
User/device/motor.h
Normal file
@ -0,0 +1,52 @@
|
||||
#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
Normal file
316
User/device/motor_lk.c
Normal file
@ -0,0 +1,316 @@
|
||||
/*
|
||||
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
Normal file
119
User/device/motor_lk.h
Normal file
@ -0,0 +1,119 @@
|
||||
#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
Normal file
478
User/device/motor_lz.c
Normal file
@ -0,0 +1,478 @@
|
||||
/*
|
||||
灵足电机驱动
|
||||
|
||||
灵足电机通信协议：
|
||||
- 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
Normal file
195
User/device/motor_lz.h
Normal file
@ -0,0 +1,195 @@
|
||||
#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
Normal file
339
User/module/balance_chassis.c
Normal file
@ -0,0 +1,339 @@
|
||||
#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,
|
||||
¤t_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, ¤t_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], ¶m->vmc_param[0], target_freq);
|
||||
VMC_Init(&c->vmc_[1], ¶m->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, ¶m->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], ¶m);
|
||||
}
|
||||
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
Normal file
194
User/module/balance_chassis.h
Normal file
@ -0,0 +1,194 @@
|
||||
/*
|
||||
* 平衡底盘模组
|
||||
*/
|
||||
|
||||
#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
Normal file
0
User/module/chassis.c
Normal file
180
User/module/chassis.h
Normal file
180
User/module/chassis.h
Normal file
@ -0,0 +1,180 @@
|
||||
/*
|
||||
* 底盘模组
|
||||
*/
|
||||
|
||||
#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
Normal file
162
User/module/config.c
Normal file
@ -0,0 +1,162 @@
|
||||
/*
|
||||
* 配置相关
|
||||
*/
|
||||
|
||||
/* 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
Normal file
35
User/module/config.h
Normal file
@ -0,0 +1,35 @@
|
||||
/*
|
||||
* 配置相关
|
||||
*/
|
||||
|
||||
#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
Normal file
662
User/module/mod_wheelleg_chassis.cpp
Normal file
@ -0,0 +1,662 @@
|
||||
#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
Normal file
57
User/task/atti_esti.c
Normal file
@ -0,0 +1,57 @@
|
||||
/*
|
||||
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
Normal file
50
User/task/blink.c
Normal file
@ -0,0 +1,50 @@
|
||||
/*
|
||||
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
Normal file
28
User/task/config.yaml
Normal file
@ -0,0 +1,28 @@
|
||||
- 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
Normal file
73
User/task/ctrl_chassis.c
Normal file
@ -0,0 +1,73 @@
|
||||
/*
|
||||
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
Normal file
49
User/task/init.c
Normal file
@ -0,0 +1,49 @@
|
||||
/*
|
||||
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
Normal file
90
User/task/rc.c
Normal file
@ -0,0 +1,90 @@
|
||||
/*
|
||||
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
Normal file
31
User/task/user_task.c
Normal file
@ -0,0 +1,31 @@
|
||||
#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
Normal file
108
User/task/user_task.h
Normal file
@ -0,0 +1,108 @@
|
||||
#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
Normal file
244
User/整合方案说明.md
Normal file
@ -0,0 +1,244 @@
|
||||
# 轮腿机器人控制系统整合方案
|
||||
|
||||
## 📋 现有系统分析
|
||||
|
||||
### 🏗️ 您的系统架构（优势）
|
||||
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. **回滚机制**：确保可以快速回到原有系统
|
||||
|
||||
这样您既可以利用现有系统的优势，又能获得新架构的清晰性和可维护性。您觉得哪个方案更适合您的需求？
|
||||
171
some_functions.c
Normal file
171
some_functions.c
Normal file
@ -0,0 +1,171 @@
|
||||
// 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
Normal file
321
utils/lqr.asv
Normal file
@ -0,0 +1,321 @@
|
||||
% v1：这份LQR程序是参考我之前写的哈工程LQR程序以及小周写的AB矩阵求解器优化后写出来的，感谢周神（2024/05/07）
|
||||
% v2：添加了可以专门调试定腿长的功能（2024/05/08）
|
||||
% v3：优化部分注释，添加单位说明（2024/05/15）
|
||||
% v4: 优化了输出，输出矩阵K的系数可以真正的复制到C里（2024/05/16）
|
||||
|
||||
% 以下所有变量含义参考2023上交轮腿电控开源（https://bbs.robomaster.com/forum.php?mod=viewthread&tid=22756）所使用符号含义
|
||||
|
||||
%%%%%%%%%%%%%%%%%%%%%%%%%Step 0：重置程序，定义变量%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
tic
|
||||
clear all
|
||||
clc
|
||||
|
||||
% 定义机器人机体参数
|
||||
syms R_w % 驱动轮半径
|
||||
syms R_l % 驱动轮轮距/2
|
||||
syms l_l l_r % 左右腿长
|
||||
syms l_wl l_wr % 驱动轮质心到左右腿部质心距离
|
||||
syms l_bl l_br % 机体质心到左右腿部质心距离
|
||||
syms l_c % 机体质心到腿部关节中心点距离
|
||||
syms m_w m_l m_b % 驱动轮质量 腿部质量 机体质量
|
||||
syms I_w % 驱动轮转动惯量 (自然坐标系法向)
|
||||
syms I_ll I_lr % 驱动轮左右腿部转动惯量 (自然坐标系法向，实际上会变化)
|
||||
syms I_b % 机体转动惯量 (自然坐标系法向)
|
||||
syms I_z % 机器人z轴转动惯量 (简化为常量)
|
||||
|
||||
% 定义其他独立变量并补充其导数
|
||||
syms theta_wl theta_wr % 左右驱动轮转角
|
||||
syms dtheta_wl dtheta_wr
|
||||
syms ddtheta_wl ddtheta_wr ddtheta_ll ddtheta_lr ddtheta_b
|
||||
|
||||
% 定义状态向量
|
||||
syms s ds phi dphi theta_ll dtheta_ll theta_lr dtheta_lr theta_b dtheta_b
|
||||
|
||||
% 定义控制向量
|
||||
syms T_wl T_wr T_bl T_br
|
||||
|
||||
% 输入物理参数：重力加速度
|
||||
syms g
|
||||
|
||||
|
||||
|
||||
%%%%%%%%%%%%%%%%%%%%%%%Step 1：解方程，求控制矩阵A，B%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
% 通过原文方程组(3.11)-(3.15)，求出ddtheta_wl,ddtheta_wr,ddtheta_ll,ddtheta_lr,ddtheta_b表达式
|
||||
eqn1 = (I_w*l_l/R_w+m_w*R_w*l_l+m_l*R_w*l_bl)*ddtheta_wl+(m_l*l_wl*l_bl-I_ll)*ddtheta_ll+(m_l*l_wl+m_b*l_l/2)*g*theta_ll+T_bl-T_wl*(1+l_l/R_w)==0;
|
||||
eqn2 = (I_w*l_r/R_w+m_w*R_w*l_r+m_l*R_w*l_br)*ddtheta_wr+(m_l*l_wr*l_br-I_lr)*ddtheta_lr+(m_l*l_wr+m_b*l_r/2)*g*theta_lr+T_br-T_wr*(1+l_r/R_w)==0;
|
||||
eqn3 = -(m_w*R_w*R_w+I_w+m_l*R_w*R_w+m_b*R_w*R_w/2)*ddtheta_wl-(m_w*R_w*R_w+I_w+m_l*R_w*R_w+m_b*R_w*R_w/2)*ddtheta_wr-(m_l*R_w*l_wl+m_b*R_w*l_l/2)*ddtheta_ll-(m_l*R_w*l_wr+m_b*R_w*l_r/2)*ddtheta_lr+T_wl+T_wr==0;
|
||||
eqn4 = (m_w*R_w*l_c+I_w*l_c/R_w+m_l*R_w*l_c)*ddtheta_wl+(m_w*R_w*l_c+I_w*l_c/R_w+m_l*R_w*l_c)*ddtheta_wr+m_l*l_wl*l_c*ddtheta_ll+m_l*l_wr*l_c*ddtheta_lr-I_b*ddtheta_b+m_b*g*l_c*theta_b-(T_wl+T_wr)*l_c/R_w-(T_bl+T_br)==0;
|
||||
eqn5 = ((I_z*R_w)/(2*R_l)+I_w*R_l/R_w)*ddtheta_wl-((I_z*R_w)/(2*R_l)+I_w*R_l/R_w)*ddtheta_wr+(I_z*l_l)/(2*R_l)*ddtheta_ll-(I_z*l_r)/(2*R_l)*ddtheta_lr-T_wl*R_l/R_w+T_wr*R_l/R_w==0;
|
||||
[ddtheta_wl,ddtheta_wr,ddtheta_ll,ddtheta_lr,ddtheta_b] = solve(eqn1,eqn2,eqn3,eqn4,eqn5,ddtheta_wl,ddtheta_wr,ddtheta_ll,ddtheta_lr,ddtheta_b);
|
||||
|
||||
|
||||
% 通过计算雅可比矩阵的方法得出控制矩阵A，B所需要的全部偏导数
|
||||
J_A = jacobian([ddtheta_wl,ddtheta_wr,ddtheta_ll,ddtheta_lr,ddtheta_b],[theta_ll,theta_lr,theta_b]);
|
||||
J_B = jacobian([ddtheta_wl,ddtheta_wr,ddtheta_ll,ddtheta_lr,ddtheta_b],[T_wl,T_wr,T_bl,T_br]);
|
||||
|
||||
% 定义矩阵A，B，将指定位置的数值根据上述偏导数计算出来并填入
|
||||
A = sym('A',[10 10]);
|
||||
B = sym('B',[10 4]);
|
||||
|
||||
% 填入A数据：a25,a27,a29,a45,a47,a49,a65,a67,a69,a85,a87,a89,a105,a107,a109
|
||||
for p = 5:2:9
|
||||
A_index = (p - 3)/2;
|
||||
A(2,p) = R_w*(J_A(1,A_index) + J_A(2,A_index))/2;
|
||||
A(4,p) = (R_w*(- J_A(1,A_index) + J_A(2,A_index)))/(2*R_l) - (l_l*J_A(3,A_index))/(2*R_l) + (l_r*J_A(4,A_index))/(2*R_l);
|
||||
for q = 6:2:10
|
||||
A(q,p) = J_A(q/2,A_index);
|
||||
end
|
||||
end
|
||||
|
||||
% A的以下数值为1：a12,a34,a56,a78,a910，其余数值为0
|
||||
for r = 1:10
|
||||
if rem(r,2) == 0
|
||||
A(r,1) = 0; A(r,2) = 0; A(r,3) = 0; A(r,4) = 0; A(r,6) = 0; A(r,8) = 0; A(r,10) = 0;
|
||||
else
|
||||
A(r,:) = zeros(1,10);
|
||||
A(r,r+1) = 1;
|
||||
end
|
||||
end
|
||||
|
||||
% 填入B数据：b21,b22,b23,b24,b41,b42,b43,b44,b61,b62,b63,b64,b81,b82,b83,b84,b101,b102,b103,b104,
|
||||
for h = 1:4
|
||||
B(2,h) = R_w*(J_B(1,h) + J_B(2,h))/2;
|
||||
B(4,h) = (R_w*(- J_B(1,h) + J_B(2,h)))/(2*R_l) - (l_l*J_B(3,h))/(2*R_l) + (l_r*J_B(4,h))/(2*R_l);
|
||||
for f = 6:2:10
|
||||
B(f,h) = J_B(f/2,h);
|
||||
end
|
||||
end
|
||||
|
||||
% B的其余数值为0
|
||||
for e = 1:2:9
|
||||
B(e,:) = zeros(1,4);
|
||||
end
|
||||
|
||||
|
||||
|
||||
%%%%%%%%%%%%%%%%%%%%%Step 2：输入参数（可以修改的部分）%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
% 物理参数赋值（唯一此处不可改变！），后面的数据通过增加后缀_ac区分模型符号和实际数据
|
||||
g_ac = 9.81;
|
||||
|
||||
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
% 此处可以输入机器人机体基本参数 %
|
||||
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
%%%%%%%%%%%%%%%%%%%%%%%%%%%机器人机体与轮部参数%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
R_w_ac = 0.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
Normal file
321
utils/lqr.m
Normal file
@ -0,0 +1,321 @@
|
||||
% v1：这份LQR程序是参考我之前写的哈工程LQR程序以及小周写的AB矩阵求解器优化后写出来的，感谢周神（2024/05/07）
|
||||
% v2：添加了可以专门调试定腿长的功能（2024/05/08）
|
||||
% v3：优化部分注释，添加单位说明（2024/05/15）
|
||||
% v4: 优化了输出，输出矩阵K的系数可以真正的复制到C里（2024/05/16）
|
||||
|
||||
% 以下所有变量含义参考2023上交轮腿电控开源（https://bbs.robomaster.com/forum.php?mod=viewthread&tid=22756）所使用符号含义
|
||||
|
||||
%%%%%%%%%%%%%%%%%%%%%%%%%Step 0：重置程序，定义变量%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
tic
|
||||
clear all
|
||||
clc
|
||||
|
||||
% 定义机器人机体参数
|
||||
syms R_w % 驱动轮半径
|
||||
syms R_l % 驱动轮轮距/2
|
||||
syms l_l l_r % 左右腿长
|
||||
syms l_wl l_wr % 驱动轮质心到左右腿部质心距离
|
||||
syms l_bl l_br % 机体质心到左右腿部质心距离
|
||||
syms l_c % 机体质心到腿部关节中心点距离
|
||||
syms m_w m_l m_b % 驱动轮质量 腿部质量 机体质量
|
||||
syms I_w % 驱动轮转动惯量 (自然坐标系法向)
|
||||
syms I_ll I_lr % 驱动轮左右腿部转动惯量 (自然坐标系法向，实际上会变化)
|
||||
syms I_b % 机体转动惯量 (自然坐标系法向)
|
||||
syms I_z % 机器人z轴转动惯量 (简化为常量)
|
||||
|
||||
% 定义其他独立变量并补充其导数
|
||||
syms theta_wl theta_wr % 左右驱动轮转角
|
||||
syms dtheta_wl dtheta_wr
|
||||
syms ddtheta_wl ddtheta_wr ddtheta_ll ddtheta_lr ddtheta_b
|
||||
|
||||
% 定义状态向量
|
||||
syms s ds phi dphi theta_ll dtheta_ll theta_lr dtheta_lr theta_b dtheta_b
|
||||
|
||||
% 定义控制向量
|
||||
syms T_wl T_wr T_bl T_br
|
||||
|
||||
% 输入物理参数：重力加速度
|
||||
syms g
|
||||
|
||||
|
||||
|
||||
%%%%%%%%%%%%%%%%%%%%%%%Step 1：解方程，求控制矩阵A，B%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
% 通过原文方程组(3.11)-(3.15)，求出ddtheta_wl,ddtheta_wr,ddtheta_ll,ddtheta_lr,ddtheta_b表达式
|
||||
eqn1 = (I_w*l_l/R_w+m_w*R_w*l_l+m_l*R_w*l_bl)*ddtheta_wl+(m_l*l_wl*l_bl-I_ll)*ddtheta_ll+(m_l*l_wl+m_b*l_l/2)*g*theta_ll+T_bl-T_wl*(1+l_l/R_w)==0;
|
||||
eqn2 = (I_w*l_r/R_w+m_w*R_w*l_r+m_l*R_w*l_br)*ddtheta_wr+(m_l*l_wr*l_br-I_lr)*ddtheta_lr+(m_l*l_wr+m_b*l_r/2)*g*theta_lr+T_br-T_wr*(1+l_r/R_w)==0;
|
||||
eqn3 = -(m_w*R_w*R_w+I_w+m_l*R_w*R_w+m_b*R_w*R_w/2)*ddtheta_wl-(m_w*R_w*R_w+I_w+m_l*R_w*R_w+m_b*R_w*R_w/2)*ddtheta_wr-(m_l*R_w*l_wl+m_b*R_w*l_l/2)*ddtheta_ll-(m_l*R_w*l_wr+m_b*R_w*l_r/2)*ddtheta_lr+T_wl+T_wr==0;
|
||||
eqn4 = (m_w*R_w*l_c+I_w*l_c/R_w+m_l*R_w*l_c)*ddtheta_wl+(m_w*R_w*l_c+I_w*l_c/R_w+m_l*R_w*l_c)*ddtheta_wr+m_l*l_wl*l_c*ddtheta_ll+m_l*l_wr*l_c*ddtheta_lr-I_b*ddtheta_b+m_b*g*l_c*theta_b-(T_wl+T_wr)*l_c/R_w-(T_bl+T_br)==0;
|
||||
eqn5 = ((I_z*R_w)/(2*R_l)+I_w*R_l/R_w)*ddtheta_wl-((I_z*R_w)/(2*R_l)+I_w*R_l/R_w)*ddtheta_wr+(I_z*l_l)/(2*R_l)*ddtheta_ll-(I_z*l_r)/(2*R_l)*ddtheta_lr-T_wl*R_l/R_w+T_wr*R_l/R_w==0;
|
||||
[ddtheta_wl,ddtheta_wr,ddtheta_ll,ddtheta_lr,ddtheta_b] = solve(eqn1,eqn2,eqn3,eqn4,eqn5,ddtheta_wl,ddtheta_wr,ddtheta_ll,ddtheta_lr,ddtheta_b);
|
||||
|
||||
|
||||
% 通过计算雅可比矩阵的方法得出控制矩阵A，B所需要的全部偏导数
|
||||
J_A = jacobian([ddtheta_wl,ddtheta_wr,ddtheta_ll,ddtheta_lr,ddtheta_b],[theta_ll,theta_lr,theta_b]);
|
||||
J_B = jacobian([ddtheta_wl,ddtheta_wr,ddtheta_ll,ddtheta_lr,ddtheta_b],[T_wl,T_wr,T_bl,T_br]);
|
||||
|
||||
% 定义矩阵A，B，将指定位置的数值根据上述偏导数计算出来并填入
|
||||
A = sym('A',[10 10]);
|
||||
B = sym('B',[10 4]);
|
||||
|
||||
% 填入A数据：a25,a27,a29,a45,a47,a49,a65,a67,a69,a85,a87,a89,a105,a107,a109
|
||||
for p = 5:2:9
|
||||
A_index = (p - 3)/2;
|
||||
A(2,p) = R_w*(J_A(1,A_index) + J_A(2,A_index))/2;
|
||||
A(4,p) = (R_w*(- J_A(1,A_index) + J_A(2,A_index)))/(2*R_l) - (l_l*J_A(3,A_index))/(2*R_l) + (l_r*J_A(4,A_index))/(2*R_l);
|
||||
for q = 6:2:10
|
||||
A(q,p) = J_A(q/2,A_index);
|
||||
end
|
||||
end
|
||||
|
||||
% A的以下数值为1：a12,a34,a56,a78,a910，其余数值为0
|
||||
for r = 1:10
|
||||
if rem(r,2) == 0
|
||||
A(r,1) = 0; A(r,2) = 0; A(r,3) = 0; A(r,4) = 0; A(r,6) = 0; A(r,8) = 0; A(r,10) = 0;
|
||||
else
|
||||
A(r,:) = zeros(1,10);
|
||||
A(r,r+1) = 1;
|
||||
end
|
||||
end
|
||||
|
||||
% 填入B数据：b21,b22,b23,b24,b41,b42,b43,b44,b61,b62,b63,b64,b81,b82,b83,b84,b101,b102,b103,b104,
|
||||
for h = 1:4
|
||||
B(2,h) = R_w*(J_B(1,h) + J_B(2,h))/2;
|
||||
B(4,h) = (R_w*(- J_B(1,h) + J_B(2,h)))/(2*R_l) - (l_l*J_B(3,h))/(2*R_l) + (l_r*J_B(4,h))/(2*R_l);
|
||||
for f = 6:2:10
|
||||
B(f,h) = J_B(f/2,h);
|
||||
end
|
||||
end
|
||||
|
||||
% B的其余数值为0
|
||||
for e = 1:2:9
|
||||
B(e,:) = zeros(1,4);
|
||||
end
|
||||
|
||||
|
||||
|
||||
%%%%%%%%%%%%%%%%%%%%%Step 2：输入参数（可以修改的部分）%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
% 物理参数赋值（唯一此处不可改变！），后面的数据通过增加后缀_ac区分模型符号和实际数据
|
||||
g_ac = 9.81;
|
||||
|
||||
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
% 此处可以输入机器人机体基本参数 %
|
||||
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
%%%%%%%%%%%%%%%%%%%%%%%%%%%机器人机体与轮部参数%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
R_w_ac = 0.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
Normal file
86
utils/matlab/LQR_calc.m
Normal file
@ -0,0 +1,86 @@
|
||||
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
Normal file
103
utils/matlab/UseBodyVel.m
Normal file
@ -0,0 +1,103 @@
|
||||
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
Normal file
39
utils/matlab/vmc.m
Normal file
@ -0,0 +1,39 @@
|
||||
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
Normal file
BIN
utils/matlab/wheel_leg.slx
Normal file
Binary file not shown.
BIN
utils/matlab/wheel_leg.slxc
Normal file
BIN
utils/matlab/wheel_leg.slxc
Normal file
Binary file not shown.
219
utils/vmc.m
Normal file
219
utils/vmc.m
Normal file
@ -0,0 +1,219 @@
|
||||
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
|
||||
Loading…
Reference in New Issue
Block a user