R2_UP/User/Module/up.h

183 lines
3.4 KiB
C
Raw Blame History

This file contains ambiguous Unicode characters

This file contains Unicode characters that might be confused with other characters. If you think that this is intentional, you can safely ignore this warning. Use the Escape button to reveal them.

#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