mirror of
https://gitee.com/dlmu-cone/bf_original_balance_chassis
synced 2026-07-23 19:25:09 +08:00
97 lines
2.6 KiB
C
97 lines
2.6 KiB
C
#pragma once
|
|
|
|
// 底盘参数
|
|
#define CALF_LEN 0.245f // 小腿
|
|
#define THIGH_LEN 0.14f // 大腿
|
|
#define JOINT_DISTANCE 0.108f // 关节间距
|
|
#define WHEEL_RADIUS 0.078f // 轮子半径
|
|
#define LIMIT_LINK_RAD 0.15149458 // 初始限位角度,见ParamAssemble
|
|
#define WHEEL_DISTANCE 0.48f // 轮子间距
|
|
#define BALANCE_GRAVITY_BIAS 0
|
|
#define ROLL_GRAVITY_BIAS 0.03f
|
|
#define MAX_ACC_REF 0.7f
|
|
#define MAX_DIST_TRACK 0.1f
|
|
#define MAX_VEL_TRACK 0.5f
|
|
|
|
#define CENTER_IMU_R 0.13f // IMU距离中心的距离
|
|
#define CENTER_IMU_W 0.11f
|
|
#define CENTER_IMU_L 0.074f
|
|
#define CENTER_IMU_H 0.060f
|
|
#define CENTER_IMU_THETA 0.9768f
|
|
|
|
#define VEL_PROCESS_NOISE 25 // 速度过程噪声
|
|
#define VEL_MEASURE_NOISE 800 // 速度测量噪声
|
|
// 同时估计加速度和速度时对加速度的噪声
|
|
// 更好的方法是设置为动态,当有冲击时/加加速度大时更相信轮速
|
|
#define ACC_PROCESS_NOISE 2000 // 加速度过程噪声
|
|
#define ACC_MEASURE_NOISE 0.01 // 加速度测量噪声
|
|
|
|
// 用于循环枚举的宏,方便访问关节电机和驱动轮电机
|
|
#define JOINT_CNT 4u
|
|
#define LF 0u
|
|
#define LB 1u
|
|
#define RF 2u
|
|
#define RB 3u
|
|
|
|
#define DRIVEN_CNT 2u
|
|
#define LD 0u
|
|
#define RD 1u
|
|
|
|
typedef struct
|
|
{
|
|
// joint
|
|
float phi1_w, phi4_w, phi2_w, phi5_w; // phi2_w used for calc real wheel speed
|
|
float T_back, T_front;
|
|
|
|
// link angle, phi1-ph5, phi5 is pod angle
|
|
float phi1, phi2, phi3, phi4, phi5;
|
|
|
|
// wheel
|
|
float w_ecd; // 电机编码器速度
|
|
float wheel_dist; // 单侧轮子的位移
|
|
float wheel_w; // 单侧轮子的速度
|
|
float body_v; // 髋关节速度
|
|
float T_wheel;
|
|
|
|
// pod
|
|
float theta, theta_w; // 杆和垂直方向的夹角,为控制状态之一
|
|
float leg_len, legd;
|
|
float height, height_v;
|
|
float F_leg, T_hip;
|
|
float target_len;
|
|
|
|
float coord[6]; // xb yb xc yc xd yd
|
|
|
|
} LinkNPodParam;
|
|
|
|
typedef struct
|
|
{
|
|
// 速度
|
|
float vel, target_v; // 底盘速度
|
|
float vel_m; // 底盘速度测量值
|
|
float vel_predict; // 底盘速度预测值
|
|
float vel_cov; // 速度方差
|
|
float acc_m, acc_last; // 水平方向加速度,用于计算速度预测值
|
|
|
|
// 位移
|
|
float dist, target_dist; // 底盘位移距离
|
|
|
|
// IMU
|
|
float yaw, wz, target_yaw; // yaw角度和底盘角速度
|
|
float pitch, pitch_w; // 底盘俯仰角度和角速度
|
|
float roll, roll_w; // 底盘横滚角度和角速度
|
|
|
|
} ChassisParam;
|
|
|
|
/**
|
|
* @brief 平衡底盘初始化
|
|
*
|
|
*/
|
|
void BalanceInit();
|
|
|
|
/**
|
|
* @brief 平衡底盘任务
|
|
*
|
|
*/
|
|
void BalanceTask();
|