288 lines
5.4 KiB
C
288 lines
5.4 KiB
C
#ifndef UP_H
|
||
#define UP_H
|
||
|
||
|
||
#include "struct_typedef.h"
|
||
#include "pid.h"
|
||
#include "bmi088.h"
|
||
#include "user_math.h"
|
||
#include "ahrs.h"
|
||
#include "can_use.h"
|
||
#include "cmd.h"
|
||
#include "filter.h"
|
||
#include "vofa.h"
|
||
#include "GO_M8010_6_Driver.h"
|
||
#include "bsp_usart.h"
|
||
|
||
/*
|
||
状态机
|
||
|
||
状态结构体，触发标志位结构体
|
||
|
||
配置层，-->config.c
|
||
|
|
||
中间层，-->up.h,统一UP_t
|
||
|
|
||
运行函数，switch(状态) 运行相应函数
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
*/
|
||
|
||
|
||
|
||
|
||
/* 投球状态定义 */
|
||
typedef enum {
|
||
PITCH_PREPARE, // 准备阶段
|
||
PITCH_START, //启动，拉扳机
|
||
PITCH_PULL_TRIGGER,
|
||
PITCH_LAUNCHING, // 发射中
|
||
PITCH_COMPLETE // 完成
|
||
} PitchState_t;
|
||
|
||
/* 投球参数配置 */
|
||
typedef struct {
|
||
fp32 m2006_init_angle; // M2006初始角度-140
|
||
fp32 m2006_trigger_angle; // M2006触发角度0,用来合并扳机
|
||
fp32 go1_init_position; // GO电机触发位置,0,初始位置
|
||
fp32 go1_release_threshold; // go上升去合并扳机，扳机位置
|
||
} PitchConfig_t;
|
||
|
||
/* 投球控制上下文 */
|
||
typedef struct {
|
||
PitchState_t PitchState;
|
||
PitchConfig_t PitchConfig;
|
||
|
||
uint8_t is_initialized ;
|
||
} PitchContext_t;
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
/*运球*/
|
||
typedef enum {
|
||
Dribble_PREPARE,
|
||
STATE_GRAB_BALL, // 夹球升起阶段
|
||
STATE_RELEASE_BALL, // 释放球体
|
||
STATE_CATCH_PREP, // 接球准备
|
||
STATE_CATCH_BALL, // 接球动作
|
||
STATE_TRANSFER, //转移球
|
||
STATE_CATCH_DONE //完成
|
||
} DribbleState_t;
|
||
|
||
/* 参数配置结构体 */
|
||
typedef struct {
|
||
fp32 m3508_init_angle; // 3508初始角度
|
||
fp32 m3508_high_angle; // 3508升起角度
|
||
fp32 go2_init_angle; // GO2初始角度
|
||
fp32 go2_flip_angle; // GO2翻转角度
|
||
fp32 flip_timing; // 翻转触发时机
|
||
// fp32
|
||
fp32 go2_release_threshold; // 释放球
|
||
} DribbleConfig_t;
|
||
|
||
/* 状态机上下文 */
|
||
typedef struct {
|
||
DribbleState_t DribbleState;
|
||
DribbleConfig_t DribbleConfig;
|
||
|
||
uint8_t is_initialized;
|
||
|
||
} DribbleContext_t;
|
||
|
||
|
||
typedef struct {
|
||
|
||
BMI088_t bmi088;
|
||
|
||
AHRS_Eulr_t imu_eulr;//解算后存放欧拉角（弧度制）
|
||
|
||
}UP_Imu_t;
|
||
|
||
/**
|
||
*@input[in] rev 暂时不知道干啥用的，github为回答issue
|
||
*@input[in] T 力矩，单位N·m
|
||
*@input[in] Pos 目标位置，单位rad
|
||
*@input[in] W 目标速度，单位rad/s
|
||
*@input[in] K_P 位置环环kp
|
||
*@input[in] K_W 速度环kp
|
||
*@note 控制公式 ： t = T + （设定位置 - 实际位置）*K_P + （设定速度 - 实际速度）*K_W
|
||
*/
|
||
typedef struct {
|
||
|
||
int rev; //暂时不知何用
|
||
float T;
|
||
float W;
|
||
float K_P;
|
||
float K_W;
|
||
|
||
}GO_param_t;
|
||
/*角度环控制电机类型*/
|
||
typedef enum {
|
||
M2006 = 1,
|
||
M3508
|
||
} MotorType_t;
|
||
|
||
|
||
|
||
typedef struct
|
||
{
|
||
|
||
pid_param_t VESC_5065_M1_param;
|
||
pid_param_t VESC_5065_M2_param;
|
||
|
||
pid_param_t UP_GM6020_speed_param;
|
||
pid_param_t UP_GM6020_angle_param;
|
||
|
||
pid_param_t M2006_speed_param;
|
||
pid_param_t M2006_angle_param;
|
||
|
||
pid_param_t M3508_speed_param;
|
||
pid_param_t M3508_angle_param;
|
||
|
||
GO_param_t go_param[2];
|
||
|
||
DribbleConfig_t DribbleConfig_Config;
|
||
PitchConfig_t PitchConfig_Config;
|
||
}UP_Param_t;
|
||
|
||
typedef struct
|
||
{
|
||
MotorType_t motor;
|
||
|
||
float orig_angle;
|
||
float last_angle;
|
||
int32_t round_cnt;
|
||
uint16_t init_cnt;
|
||
float total_angle;
|
||
|
||
}motor_angle_data;
|
||
|
||
|
||
typedef struct{
|
||
|
||
uint8_t up_task_run;
|
||
const UP_Param_t *param;
|
||
|
||
int state;
|
||
|
||
|
||
|
||
/*运球过程*/
|
||
DribbleContext_t DribbleContext;
|
||
/*投篮过程*/
|
||
PitchContext_t PitchContext;
|
||
|
||
|
||
|
||
|
||
UP_Imu_t pos088;
|
||
|
||
CMD_t *cmd;
|
||
|
||
struct{
|
||
fp32 rotor_pit6020ecd;
|
||
fp32 rotor_pit6020rpm;
|
||
|
||
fp32 VESC_5065_M1_rpm;
|
||
fp32 VESC_5065_M2_rpm;
|
||
|
||
motor_angle_data M2006;
|
||
motor_angle_data M3508;
|
||
|
||
GO_Motorfield *GO_motor_info[GO_NUM];//存放go电机数据
|
||
|
||
fp32 M3508_angle[4];
|
||
fp32 M3508_rpm [4];
|
||
|
||
int flag;
|
||
|
||
}motorfeedback;
|
||
|
||
|
||
|
||
struct{
|
||
|
||
fp32 rotor_pit6020angle;
|
||
|
||
fp32 VESC_5065_M1_rpm;
|
||
fp32 VESC_5065_M2_rpm;
|
||
|
||
fp32 M2006_angle;
|
||
fp32 M3508_angle;
|
||
|
||
fp32 go_shoot;
|
||
fp32 go_spin;
|
||
|
||
}motor_target;
|
||
|
||
struct{
|
||
|
||
pid_type_def GM6020_speed;
|
||
pid_type_def GM6020_angle;
|
||
|
||
pid_type_def VESC_5065_M1;
|
||
pid_type_def VESC_5065_M2;
|
||
|
||
pid_type_def M2006_angle;
|
||
pid_type_def M2006_speed;
|
||
|
||
pid_type_def M3508_angle;
|
||
pid_type_def M3508_speed;
|
||
|
||
}pid;
|
||
|
||
|
||
|
||
/*经PID计算后的实际发送给电机的实时输出值*/
|
||
struct
|
||
{
|
||
fp32 final_3508out[4];
|
||
fp32 final_pitchout;
|
||
fp32 final_VESC_5065_M1out;
|
||
fp32 final_VESC_5065_M2out;
|
||
|
||
|
||
}final_out;
|
||
|
||
LowPassFilter2p_t filled[6]; /* 输出滤波器滤波器数组 */
|
||
|
||
fp32 vofa_send[8];
|
||
|
||
|
||
|
||
} UP_t;
|
||
|
||
|
||
int8_t up_init(UP_t *u,const UP_Param_t *param,float target_freq);
|
||
|
||
int8_t UP_UpdateFeedback(UP_t *u, const CAN_t *can, CMD_t *c) ;
|
||
int8_t GM6020_control(UP_t *u,fp32 angle);
|
||
int8_t VESC_M5065_Control(UP_t *u,fp32 speed);
|
||
int8_t GO_SendData(UP_t *u,int id,float pos);
|
||
int8_t UP_angle_control(UP_t *u, fp32 target_angle,MotorType_t motor);
|
||
int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c);
|
||
int8_t ALL_Motor_Control(UP_t *u,CAN_Output_t *out);
|
||
int8_t UP_M2006_angle(UP_t *u,fp32 target_angle);
|
||
int8_t UP_M3508_speed(UP_t *u,fp32 speed);
|
||
int8_t Pitch_Process(UP_t *u, CAN_Output_t *out);
|
||
|
||
int8_t Dribble_Process(UP_t *u, CAN_Output_t *out);
|
||
|
||
void Dribbl_transfer_a(UP_t *u, CAN_Output_t *out);
|
||
|
||
#endif
|