293 lines
6.7 KiB
C
293 lines
6.7 KiB
C
#pragma once
|
||
|
||
#ifdef __cplusplus
|
||
extern "C"
|
||
{
|
||
#endif
|
||
|
||
#include "bsp/struct_typedef.h"
|
||
#include "component/filter.h"
|
||
#include "component/pid.h"
|
||
#include "component/ahrs.h"
|
||
#include "device/buzzer.h"
|
||
#include "device/bmi088.h"
|
||
#include "component/user_math.h"
|
||
#include "component/filter.h"
|
||
#include "device/motor_rm.h"
|
||
|
||
#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 */
|
||
|
||
// m3508的电机转速转换为底盘的实际速度
|
||
#define M3508_MOTOR_RPM_TO_VECTOR 0.0008809748903494517209f
|
||
|
||
#define M6020_MOTOR_RPM_TO_VECTOR 0.003664f
|
||
|
||
// 纵向/横向
|
||
#define radians atan(1.0f * 330 /330) // 角度制
|
||
|
||
// 四个舵轮的安装误差
|
||
typedef struct
|
||
{
|
||
fp32 MOTOR_OFFSET[4];
|
||
} MotorOffset_t;
|
||
|
||
/*底盘的类型*/
|
||
typedef enum
|
||
{
|
||
CHASSIS_TYPE_MECANUM, /* 麦轮 */
|
||
CHASSIS_TYPE_OMNI_CROSS, /* 全向轮*/
|
||
CHASSIS_TYPE_AGV_3, /* AGV舵轮 */
|
||
CHASSIS_TYPE_AGV_4, /* AGV舵轮 */
|
||
} Chassis_Type_e;
|
||
|
||
/*底盘的电机轮组*/
|
||
typedef enum
|
||
{
|
||
DJI_M3508,
|
||
DJI_G6020,
|
||
AGV_Group,
|
||
} Chassis_Motortype_e;
|
||
|
||
/*底盘模式*/
|
||
typedef enum
|
||
{
|
||
STOP, // 底盘平行
|
||
RC, // 遥控模式
|
||
LOCK, //底盘锁死
|
||
} Chassis_mode_t;
|
||
// typedef struct
|
||
// {
|
||
|
||
// Chassis_mode_t mode; // 底盘模式
|
||
|
||
// } Chassis_CMD__t;
|
||
|
||
typedef struct
|
||
{
|
||
int cmd_power_on_safe; // 上电安全标志
|
||
|
||
Chassis_mode_t mode;
|
||
|
||
// 遥控器输出值
|
||
fp32 Vx;
|
||
fp32 Vy;
|
||
fp32 Vw;
|
||
fp32 throttle;
|
||
|
||
} Chassis_CMD_t;
|
||
|
||
typedef struct
|
||
{
|
||
|
||
BMI088_t bmi088;
|
||
|
||
/*可通过该枚举类型来决定Imu的数据量纲*/
|
||
enum
|
||
{
|
||
IMU_DEGREE, // 角度值（0-360）
|
||
IMU_RADIAN // 弧度制（0-2pi)
|
||
} angle_mode;
|
||
|
||
AHRS_Eulr_t imu_eulr; // 解算后存放欧拉角（弧度制）
|
||
} ChassisImu_t;
|
||
|
||
/*底盘参数结构体*/
|
||
typedef struct
|
||
{
|
||
|
||
//Chassis_Type_e chassis_type; /* */
|
||
//Chassis_Motortype_e motor_type; /**/
|
||
|
||
/*该部分决定PID的参数整定在config中修改*/
|
||
KPID_Params_t C6020Omega_param;
|
||
KPID_Params_t C6020Angle_param;
|
||
KPID_Params_t M3508v_param;
|
||
|
||
// KPID_Params_t Chassis_AngleAdjust_param;
|
||
// KPID_Params_t RadarAngle_param;
|
||
// KPID_Params_t RadarSpeed_param;
|
||
// KPID_Params_t SickVx_param;
|
||
// KPID_Params_t SickVy_param;
|
||
// KPID_Params_t SickVw_param;
|
||
|
||
// KPID_Params_t M3508_param;
|
||
// KPID_Params_t C6020pitAngle_param;
|
||
// KPID_Params_t C6020pitOmega_param;
|
||
|
||
MOTOR_RM_Param_t motor_3508_param[4]; // 四个3508电机
|
||
MOTOR_RM_Param_t motor_6020_param[4]; // 四个6020电机
|
||
|
||
} Chassis_Param_t;
|
||
|
||
typedef struct
|
||
{
|
||
fp32 Vx;
|
||
fp32 Vy;
|
||
fp32 Vw;
|
||
fp32 mul; // 油门倍率
|
||
} ChassisMove_Vec;
|
||
|
||
typedef struct
|
||
{
|
||
float rotor3508_out[4];
|
||
float rotor6020_out[4];
|
||
} Chassis_out_t;
|
||
|
||
typedef struct
|
||
{
|
||
uint64_t last_wakeup;
|
||
float dt;
|
||
|
||
Chassis_mode_t mode;
|
||
ChassisMove_Vec move_vec; // 最终输入速度
|
||
|
||
/*期望的底盘输出值(此处为舵轮解算出的各个电机的期望输出值)ֵ*/
|
||
struct
|
||
{
|
||
fp32 rotor3508_jiesuan_1[4];
|
||
fp32 rotor3508_jiesuan_2[4];
|
||
fp32 rotor6020_jiesuan_1[4];
|
||
fp32 rotor6020_jiesuan_2[4];
|
||
// fp32 rotor6020_elur_yaw;
|
||
|
||
fp32 motor6020_target[4];
|
||
fp32 motor3508_target[4];
|
||
|
||
} hopemotorout;
|
||
|
||
struct
|
||
{
|
||
fp32 final_6020out[4];
|
||
fp32 final_3508out[4];
|
||
// fp32 final_pitchout;
|
||
} final_out;
|
||
|
||
struct
|
||
{
|
||
fp32 rotor_rpm3508[4];
|
||
fp32 rotor_current3508[4];
|
||
fp32 rotor_rpm6020[4];
|
||
fp32 rotor_angle6020[4];
|
||
fp32 rotor_current6020[4];
|
||
fp32 rotor_temp6020[4];
|
||
} motorfeedback;
|
||
|
||
struct
|
||
{
|
||
KPID_t chassis_6020anglePid[4];
|
||
KPID_t chassis_6020OmegaPid[4];
|
||
KPID_t chassis_3508VPID[4];
|
||
|
||
} 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校准数据
|
||
Chassis_Param_t *param; // 一些固定的参数
|
||
fp32 vofa_send[8]; // vofa输出数据
|
||
LowPassFilter2p_t filled[3]; // 低通滤波器
|
||
float keep_angle[4]; // 保持的 6020 角度
|
||
|
||
Chassis_out_t out;
|
||
} Chassis_t;
|
||
|
||
// typedef struct
|
||
// {
|
||
|
||
// uint64_t last_wakeup;
|
||
// float dt;
|
||
|
||
// Chassis_mode_t mode;
|
||
// // CMD_Chassis_navi_pos pos;
|
||
|
||
// ChassisMove_Vec move_vec; // 最终输入速度
|
||
|
||
// /*期望的底盘输出值(此处为舵轮解算出的各个电机的期望输出值)ֵ*/
|
||
// struct
|
||
// {
|
||
// fp32 rotor3508_jiesuan_1[4];
|
||
// fp32 rotor3508_jiesuan_2[4];
|
||
// fp32 rotor6020_jiesuan_1[4];
|
||
// fp32 rotor6020_jiesuan_2[4];
|
||
// fp32 rotor6020_elur_yaw;
|
||
|
||
// fp32 motor6020_target[4];
|
||
// fp32 motor3508_target[4];
|
||
|
||
// } hopemotorout;
|
||
|
||
// /*经PID计算后的实际发送给电机的实时输出值*/
|
||
// struct
|
||
// {
|
||
// fp32 final_6020out[4];
|
||
// // fp32 final_5065out[4];
|
||
// fp32 final_3508out[4];
|
||
// fp32 final_pitchout;
|
||
// } final_out;
|
||
|
||
// /*电机反馈数据*/
|
||
// struct
|
||
// {
|
||
// fp32 rotor_rpm3508[4];
|
||
// fp32 rotor_current3508[4];
|
||
|
||
// fp32 rotor_rpm6020[4];
|
||
// fp32 rotor_angle6020[4];
|
||
// fp32 rotor_current6020[4];
|
||
// fp32 rotor_temp6020[4];
|
||
|
||
// } motorfeedback;
|
||
|
||
// /*pid*/
|
||
// struct
|
||
// {
|
||
// KPID_t chassis_6020anglePid[4];
|
||
// KPID_t chassis_6020OmegaPid[4];
|
||
// // KPID_t Chassis_AngleAdjust;
|
||
// // KPID_t chassis_RadarspeedPID[2];
|
||
// // KPID_t chassis_SickVx;
|
||
// // KPID_t chassis_SickVy;
|
||
// // KPID_t chassis_SickVw;
|
||
|
||
// KPID_t chassis_3508VPID[4];
|
||
// // KPID_t chassis_pitAngle6020;
|
||
// // KPID_t chassis_pitOmega6020;
|
||
// } pid;
|
||
|
||
// // fp32 chassis_yaw; // 码盘世界坐标系下底盘角度
|
||
// 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校准数据
|
||
// Chassis_Param_t *param; // 一些固定的参数
|
||
// fp32 vofa_send[8]; // vofa输出数据
|
||
// LowPassFilter2p_t filled[9]; // 低通滤波器
|
||
// float keep_angle[4]; // 保持的 6020 角度
|
||
|
||
// // BUZZER_t buzzer_radar_angle; // 用于雷达角度纠正的蜂鸣器控制
|
||
// // BUZZER_t buzzer_nuc_flag; // 用于nuc校准成功的蜂鸣器控制
|
||
|
||
// } 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);
|
||
void Chassis_Setoutput(Chassis_t *c);
|
||
#ifdef __cplusplus
|
||
}
|
||
#endif
|