241 lines
5.4 KiB
C
241 lines
5.4 KiB
C
#pragma once
|
||
|
||
#ifdef __cplusplus
|
||
extern "C"
|
||
{
|
||
#endif
|
||
|
||
#include "struct_typedef.h"
|
||
#include "component/filter.h"
|
||
#include "component/pid.h"
|
||
#include "component/ahrs.h"
|
||
//#include "device/buzzer.h"
|
||
#include "component/user_math.h"
|
||
#include "component/filter.h"
|
||
#include "device/motor_rm.h"
|
||
|
||
typedef struct {
|
||
float max_velocity;
|
||
float acceleration;
|
||
float deceleration;
|
||
} TrapezoidalProfile;
|
||
|
||
#define CHASSIS_OK (0)
|
||
#define CHASSIS_ERR (-1)
|
||
#define CHASSIS_ERR_NULL (-2)
|
||
#define CHASSIS_ERR_MODE (-3) /*CMD_ChassisMode_t */
|
||
#define CHASSIS_ERR_TYPE (-4) /*Chassis_Type_t */
|
||
|
||
|
||
// 纵向/横向
|
||
#define radians atan(1.0f * 390 /390) // 角度制
|
||
|
||
|
||
/*底盘姿态模式*/
|
||
typedef enum
|
||
{
|
||
STOP_MODE,/* 急停模式 */
|
||
EXPAND_MODE,/* 展开模式 */
|
||
REDUCE_MODE,/* 收缩模式 */
|
||
} CHASSIS_ATTITUDE_MODE_t;
|
||
|
||
/*底盘状态*/
|
||
typedef enum
|
||
{
|
||
STOP_STATE,/* 急停状态 */
|
||
EXPAND_STATE,/* 展开状态 */
|
||
REDUCE_STATE,/* 收缩状态 */
|
||
} CHASSIS_STATE_t;
|
||
|
||
/*底盘模式*/
|
||
typedef enum
|
||
{
|
||
STOP, // 底盘平行
|
||
RC, // 遥控模式
|
||
CHASSIS_MODE_BREAK, /* 刹车模式，电机闭环控制保持静止。用于机器人停止状态 */
|
||
CHASSIS_MODE_RELAX, /* 放松模式，电机不输出。一般情况底盘初始化之后的模式 */
|
||
CHASSIS_MODE_FOLLOW_GIMBAL, /* 通过闭环控制使车头方向跟随云台 */
|
||
CHASSIS_MODE_ROTOR, /* 小陀螺模式，通过闭环控制使底盘不停旋转 */
|
||
|
||
} Chassis_Mode_t;
|
||
|
||
typedef struct
|
||
{
|
||
int cmd_power_on_safe; // 上电安全标志
|
||
Chassis_Mode_t mode;
|
||
CHASSIS_ATTITUDE_MODE_t attitude_mode;
|
||
// 遥控器输出值
|
||
fp32 Vx;
|
||
fp32 Vy;
|
||
fp32 Vw;
|
||
fp32 throttle;
|
||
|
||
fp32 x_l;
|
||
fp32 y_l;
|
||
} Chassis_CMD_t;
|
||
|
||
// 四个舵轮的安装误差
|
||
typedef struct
|
||
{
|
||
fp32 MOTOR_OFFSET[4];
|
||
} MotorOffset_t;
|
||
|
||
typedef struct
|
||
{
|
||
fp32 TELESCOPE_MOTOR_OFFSET[2];
|
||
} TelescopeMotorOffset_t;
|
||
|
||
typedef struct
|
||
{
|
||
/*可通过该枚举类型来决定Imu的数据量纲*/
|
||
enum
|
||
{
|
||
IMU_DEGREE, // 角度值（0-360）
|
||
IMU_RADIAN // 弧度制（0-2pi)
|
||
} angle_mode;
|
||
|
||
// AHRS_Eulr_t imu_eulr; // 解算后存放欧拉角（弧度制）
|
||
} ChassisImu_t;
|
||
|
||
/*底盘参数结构体*/
|
||
typedef struct
|
||
{
|
||
|
||
|
||
|
||
|
||
struct{
|
||
/*该部分决定PID的参数整定在config中修改*/
|
||
KPID_Params_t Radder_DIR_Omega;
|
||
KPID_Params_t Radder_DIR_Angle;
|
||
KPID_Params_t Wheel_DIR_Omega;
|
||
KPID_Params_t chassis_follow_gimbal;
|
||
}pid;
|
||
|
||
struct{
|
||
MOTOR_RM_Param_t Wheel_DIR[4]; // 四个轮向电机
|
||
MOTOR_RM_Param_t Radder_DIR[4]; // 四个舵向电机
|
||
MOTOR_RM_Param_t chassis_follow_gimbal; // 底盘跟随云台
|
||
|
||
}motor;
|
||
float mech_zero;/*云台6020的机械中点*/
|
||
float travel ;/*云台6020的机械行程*/
|
||
float mech_zero_4310;/*云台4310的机械中点*/
|
||
} Chassis_Param_t;
|
||
|
||
typedef struct
|
||
{
|
||
fp32 Vx;
|
||
fp32 Vy;
|
||
fp32 Vw;
|
||
fp32 mul; // 油门倍率
|
||
} ChassisMove_Vec;
|
||
|
||
typedef struct
|
||
{
|
||
float Wheel_DIR[4];
|
||
float Radder_DIR[4];
|
||
} Chassis_out_t;
|
||
|
||
typedef struct
|
||
{
|
||
|
||
uint32_t last_wakeup;
|
||
float dt;
|
||
float chassis6020_detangle[4];
|
||
Chassis_Mode_t mode;
|
||
ChassisMove_Vec move_vec; // 最终输入速度
|
||
TrapezoidalProfile telescope_profile;
|
||
/* 设立角度和跟随角度 */
|
||
float Set_TelescopeAngle;
|
||
float Last_Set_TelescopeAngle;
|
||
float Follow_TelescopeAngle;
|
||
float feedfrward;
|
||
float Telescope_err;
|
||
/*期望的底盘输出值(此处为舵轮解算出的各个电机的期望输出值)ֵ*/
|
||
struct{
|
||
uint32_t last_wakeup;
|
||
float dt;
|
||
}accl_time;
|
||
|
||
struct
|
||
{
|
||
fp32 Wheel_DIR_Solving_1[4];
|
||
fp32 Wheel_DIR_Solving_2[4];
|
||
fp32 Radder_DIR_Solving_1[4];
|
||
fp32 Radder_DIR_Solving_2[4];
|
||
// fp32 rotor6020_elur_yaw;
|
||
|
||
fp32 Radder_DIR_target[4];
|
||
fp32 Wheel_DIR_target[4];
|
||
|
||
} hopemotorout;
|
||
|
||
struct
|
||
{
|
||
fp32 final_Telescope;
|
||
fp32 final_Radder_DIR[4];
|
||
fp32 final_Wheel_DIR[4];
|
||
} final_out;
|
||
/* 原始数据 */
|
||
struct
|
||
{
|
||
struct{
|
||
|
||
MOTOR_Feedback_t Wheel_DIR[4]; // 四个轮向电机
|
||
MOTOR_Feedback_t Radder_DIR[4];
|
||
MOTOR_Feedback_t gimbal_yaw;
|
||
}motor;
|
||
/* 转化数据 */
|
||
struct{
|
||
|
||
float Radder_DIR_Angle[4];
|
||
float Radder_DIR_Omega[4];
|
||
float Wheel_DIR_Angle[4];
|
||
float Wheel_DIR_Omega[4];
|
||
}motor_transformation;
|
||
|
||
} feedback;
|
||
|
||
struct
|
||
{
|
||
KPID_t Radder_DIR_angle[4];
|
||
KPID_t Radder_DIR_omega[4];
|
||
KPID_t Wheel_DIR_omega[4];
|
||
KPID_t chassis_follow_gimbal_pid;
|
||
} pid;
|
||
|
||
uint8_t keeping_angle_flag; // 是否处于保持角度模式
|
||
|
||
// AHRS_Eulr_t set_point; // 底盘纠正目标角
|
||
// fp32 angle_current; // 当前角度
|
||
// fp32 angle_piancha; // 偏差角度
|
||
// fp32 yaw_out; // 角度pid输出值
|
||
|
||
ChassisImu_t pos088; // 088的实时姿态
|
||
MotorOffset_t motoroffset; // 5065校准数据
|
||
TelescopeMotorOffset_t telescope_motor_offset;
|
||
|
||
|
||
Chassis_Param_t *param; // 一些固定的参数
|
||
LowPassFilter2p_t filled[11]; // 低通滤波器
|
||
LowPassFilter2p_t accl_filled[2]; // 加速度滤波器
|
||
float keep_angle[4]; // 保持的 舵向 角度
|
||
|
||
Chassis_out_t out;
|
||
|
||
float wz_multi; /* 小陀螺模式旋转方向 */
|
||
float mech_zero;/*云台6020的机械中点*/
|
||
float travel ;/*云台6020的机械行程*/
|
||
// float mech_zero_4310;/*云台4310的机械中点*/
|
||
} Chassis_t;
|
||
|
||
|
||
int8_t chassis_init(Chassis_t *c, Chassis_Param_t *param, float target_freq);
|
||
int8_t Chassis_update(Chassis_t *c);
|
||
int8_t Chassis_Control(Chassis_t *c, Chassis_CMD_t *c_cmd,uint32_t now);
|
||
void Chassis_Setoutput(Chassis_t *c);
|
||
#ifdef __cplusplus
|
||
}
|
||
#endif
|