#ifndef UP_H #define UP_H #include "struct_typedef.h" #include "pid.h" #include "bmi088.h" #include "map.h" #include "user_math.h" #include "ahrs.h" #include "can_use.h" #include "cmd.h" #include "filter.h" #include "Action.h" #include "chassis.h" #include "vofa.h" #include "GO_M8010_6_Driver.h" typedef enum { STATE_IDLE, // 空闲状态 STATE_PRE_DRIBBLE, // 运球前 STATE_POST_DRIBBLE, // 运球后 STATE_PRE_LAUNCH, // 发射前 STATE_POST_LAUNCH // 发射后 } OperationState; 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的参数整定在config中修改*/ 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; GO_param_t go_param; }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; UP_Imu_t pos088; OperationState ctrl; motor_angle_data M2006; motor_angle_data M3508; GO_Motorfield *GO_motor_info[GO_NUM];//存放go电机数据 struct{ fp32 rotor_pit6020ecd; fp32 rotor_pit6020rpm; fp32 VESC_5065_M1_rpm; fp32 VESC_5065_M2_rpm; fp32 M2006_rpm; fp32 M2006_angle; fp32 M3508_speed[3]; }motorfeedback; struct{ fp32 rotor_pit6020angle; fp32 VESC_5065_M1_rpm; fp32 VESC_5065_M2_rpm; fp32 M2006_angle; fp32 M3508_speed; }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_speed[3]; }pid; /*经PID计算后的实际发送给电机的实时输出值*/ struct { fp32 final_2006out; fp32 final_3508out[3]; 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) ; 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(UP_t *u, fp32 target_angle) ; int8_t UP_control(UP_t *u,CAN_Output_t *out); 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); #endif