#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