281 lines
6.3 KiB
C
281 lines
6.3 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 {
|
||
CoPREPARE, // 准备阶段
|
||
CoTRANSLATE_OUT,//上方平移,去运球
|
||
CoDRIBBLE, //运球和蓄力阶段,再平移回去
|
||
CoTRANSLATE_IN,//上方平移，送球发射，回到运球位置
|
||
CoPITCH, //发射
|
||
CoPITCH_MID360, //雷达发射
|
||
CoDONE,
|
||
|
||
|
||
}CoState_t;
|
||
/*总配合上下文*/
|
||
typedef struct {
|
||
CoState_t CoState;
|
||
|
||
uint8_t is_init ;
|
||
} CoCon_t;
|
||
|
||
|
||
|
||
|
||
/* 投球状态定义 */
|
||
typedef enum {
|
||
PITCH_PREPARE, // 准备阶段
|
||
PITCH_START, //启动，拉扳机
|
||
PITCH_PULL_TRIGGER,
|
||
PITCH_LAUNCHING, // 发射中
|
||
|
||
PITCH_COMPLETE // 完成
|
||
|
||
} PitchState_t;
|
||
|
||
/* 投球参数配置 */
|
||
typedef struct {
|
||
fp32 m2006_init; // M2006初始角度
|
||
fp32 m2006_trig; // M2006触发角度0,用来合并扳机
|
||
fp32 go_init; // GO电机触发位置,初始位置，后续更改用于发射的位置
|
||
fp32 go_pull_pos; // go上升去合并扳机，扳机位置
|
||
fp32 Pitch_angle;
|
||
|
||
fp32 pull_speed;//go速度
|
||
fp32 go_release_pos;//go松开，发射位置
|
||
|
||
} PitchCfg_t;
|
||
//fp32 go_pull_pos;
|
||
// fp32 m2006_init; // M2006初始角度
|
||
// fp32 m2006_trig; // M2006触发角度
|
||
// fp32 go_init; // GO初始位置
|
||
// fp32 go_release; // GO释放阈值
|
||
// fp32 go_recv; // GO接球位置
|
||
// fp32 screw_init; // 螺杆初始值
|
||
// fp32 pitch_ang; // 投球角度
|
||
// fp32 pull_spd; // 拉动速度
|
||
/* 投球控制上下文 */
|
||
typedef struct {
|
||
PitchState_t PitchState;
|
||
PitchCfg_t PitchConfig;
|
||
|
||
uint8_t is_init ;
|
||
} Pitch_Cfg_t;
|
||
|
||
/*运球*/
|
||
typedef enum {
|
||
DRIBBLE_PREPARE, // 准备阶段
|
||
DRIBBLE_START,
|
||
DRIBBLE_TRANSLATE, // 平移过程
|
||
DRIBBLE_PROCESS_DOWN, // 运球过程，球下落
|
||
DRIBBLE_PROCESS_UP, // 运球过程，球上升
|
||
DRIBBLE_DONE // 运球结束
|
||
} DribbleState_t;
|
||
|
||
/* 参数配置结构体 */
|
||
typedef struct {
|
||
int8_t dribble_flag;//当移动三摩擦后为1，否则为0，防止发射撞到
|
||
|
||
fp32 m3508_init_ang; // 3508平移前位置
|
||
fp32 m3508_translate_angle; // 平移后位置
|
||
fp32 init_spd;
|
||
fp32 spd; // 转动速度
|
||
fp32 rev_spd;
|
||
|
||
|
||
/*光电标志位，初始值均为0，触发为1*/
|
||
int light_3508_flag;//3508平移处的光电，0初始，1到位置
|
||
int light_ball_flag;//检测球的flag
|
||
|
||
} DribbleCfg_t;
|
||
|
||
/* 状态机上下文 */
|
||
typedef struct {
|
||
DribbleState_t DribbleState;
|
||
DribbleCfg_t DribbleConfig;
|
||
|
||
uint8_t is_init;
|
||
|
||
} DribbleCont_t;
|
||
|
||
|
||
typedef struct {
|
||
|
||
BMI088_t bmi088;
|
||
|
||
AHRS_Eulr_t imu_eulr;//解算后存放欧拉角（弧度制）
|
||
|
||
}UP_Imu_t;
|
||
|
||
typedef struct
|
||
{
|
||
|
||
pid_param_t VESC_5065_M1_param;
|
||
pid_param_t VESC_5065_M2_param;
|
||
|
||
pid_param_t Shoot_M2006_speed_param;
|
||
pid_param_t Shoot_M2006_angle_param;
|
||
|
||
pid_param_t Pitch_M2006_speed_param;
|
||
pid_param_t Pitch_M2006_angle_param;
|
||
|
||
pid_param_t Pitch_Angle_M2006_speed_param;
|
||
|
||
pid_param_t Dribble_M3508_speed_param;//三摩擦用的速度环
|
||
|
||
pid_param_t Dribble_translate_M3508_speed_param;//平移用的速度环
|
||
pid_param_t Dribble_translate_M3508_angle_param;//平移用的角度环
|
||
|
||
|
||
DribbleCfg_t DribbleCfg;
|
||
PitchCfg_t PitchCfg;
|
||
|
||
GO_MotorCmd_t go_cmd;
|
||
}UP_Param_t;
|
||
typedef struct{
|
||
|
||
fp32 VESC_5065_M1_rpm;
|
||
fp32 VESC_5065_M2_rpm;
|
||
|
||
fp32 Shoot_M2006_angle;
|
||
fp32 Pitch_M2006_angle;
|
||
|
||
fp32 go_shoot;
|
||
fp32 Dribble_M3508_speed[3];//运球转速
|
||
fp32 Dribble_translate_M3508_angle;//平移用的3508转动角度
|
||
|
||
fp32 Shoot_Pitch_angle;
|
||
|
||
}up_motor_target_t;
|
||
|
||
typedef struct{
|
||
|
||
|
||
pid_type_def VESC_5065_M1;
|
||
pid_type_def VESC_5065_M2;
|
||
|
||
pid_type_def Shoot_M2006angle;
|
||
pid_type_def Shoot_M2006speed;
|
||
|
||
pid_type_def Pitch_M2006_angle;
|
||
pid_type_def Pitch_M2006_speed;
|
||
|
||
pid_type_def Pitch_Angle_M2006_speed;
|
||
|
||
pid_type_def Dribble_M3508_speed[3];//三摩擦用的速度环
|
||
|
||
pid_type_def Dribble_translate_M3508_speed;//平移用的速度环
|
||
pid_type_def Dribble_translate_M3508_angle;//平移用的角度环
|
||
|
||
|
||
}up_pid_t;
|
||
typedef struct
|
||
{
|
||
fp32 ecd;
|
||
fp32 rpm;
|
||
uint8_t id;
|
||
fp32 orig_angle;
|
||
fp32 last_angle;
|
||
int32_t round_cnt;
|
||
int init_cnt;
|
||
fp32 total_angle;
|
||
}DJmotor_feedback_t;
|
||
|
||
|
||
typedef struct{
|
||
|
||
uint8_t up_task_run;
|
||
const UP_Param_t *param;
|
||
CoCon_t CoContext;
|
||
/*运球过程*/
|
||
DribbleCont_t DribbleCont;
|
||
/*投篮过程*/
|
||
Pitch_Cfg_t Pitch_Cfg;
|
||
|
||
CMD_t *cmd;
|
||
|
||
struct{
|
||
|
||
struct{
|
||
fp32 VESC_5065_M1_rpm;
|
||
fp32 VESC_5065_M2_rpm;
|
||
}VESC;
|
||
GO_MotorData_t *go_data;//存放go电机数据
|
||
|
||
DJmotor_feedback_t DJmotor_feedback[6];
|
||
|
||
struct {
|
||
uint32_t ecd;
|
||
float angle;
|
||
}Encoder;
|
||
}motorfeedback;
|
||
|
||
|
||
|
||
|
||
up_pid_t pid;
|
||
up_motor_target_t motor_target;
|
||
|
||
GO_MotorCmd_t go_cmd;
|
||
|
||
/*经PID计算后的实际发送给电机的实时输出值*/
|
||
struct
|
||
{
|
||
fp32 DJfinal_out[6];
|
||
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 VESC_M5065_Control(UP_t *u,fp32 speed);
|
||
int8_t GO_SendData(UP_t *u,float pos,float limit);
|
||
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 DJ_processdata(DJmotor_feedback_t *f,fp32 ecd_to_angle);
|
||
int8_t DJ_Angle_Control(UP_t *u,int id,DJmotor_feedback_t *f,pid_type_def *Angle_pid,pid_type_def *Speed_pid,fp32 target_angle);
|
||
int8_t DJ_Speed_Control(UP_t *u,int id,DJmotor_feedback_t *f,pid_type_def *Speed_pid,fp32 target_speed);
|
||
|
||
int8_t Dribble_Process(UP_t *u, CAN_Output_t *out);
|
||
int8_t Pitch_Process(UP_t *u, CAN_Output_t *out);
|
||
int8_t Co_Process(UP_t *u, CAN_Output_t *out);
|
||
|
||
|
||
#endif
|