2025-03-12 10:46:02 +08:00
|
|
|
|
#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;
|
|
|
|
|
|
2025-03-17 00:32:06 +08:00
|
|
|
|
/**
|
|
|
|
|
*@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;
|
2025-03-12 10:46:02 +08:00
|
|
|
|
|
2025-03-17 00:32:06 +08:00
|
|
|
|
}GO_param_t;
|
|
|
|
|
|
|
|
|
|
typedef enum {
|
|
|
|
|
M2006 = 1,
|
|
|
|
|
M3508
|
|
|
|
|
} MotorType_t;
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
2025-03-12 10:46:02 +08:00
|
|
|
|
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;
|
|
|
|
|
|
2025-03-17 00:32:06 +08:00
|
|
|
|
pid_param_t M3508_speed_param;
|
|
|
|
|
GO_param_t go_param;
|
|
|
|
|
|
2025-03-12 10:46:02 +08:00
|
|
|
|
}UP_Param_t;
|
|
|
|
|
|
|
|
|
|
typedef struct
|
|
|
|
|
{
|
2025-03-17 00:32:06 +08:00
|
|
|
|
MotorType_t motor;
|
|
|
|
|
float orig_angle;
|
|
|
|
|
float last_angle;
|
|
|
|
|
int32_t round_cnt;
|
|
|
|
|
uint16_t init_cnt;
|
|
|
|
|
float total_angle;
|
2025-03-12 10:46:02 +08:00
|
|
|
|
|
2025-03-17 00:32:06 +08:00
|
|
|
|
}motor_angle_data;
|
2025-03-12 10:46:02 +08:00
|
|
|
|
|
|
|
|
|
|
|
|
|
|
typedef struct{
|
|
|
|
|
|
|
|
|
|
uint8_t up_task_run;
|
|
|
|
|
const UP_Param_t *param;
|
|
|
|
|
UP_Imu_t pos088;
|
|
|
|
|
|
|
|
|
|
OperationState ctrl;
|
2025-03-17 00:32:06 +08:00
|
|
|
|
motor_angle_data M2006;
|
|
|
|
|
motor_angle_data M3508;
|
|
|
|
|
GO_Motorfield *GO_motor_info[GO_NUM];//存放go电机数据
|
|
|
|
|
|
2025-03-12 10:46:02 +08:00
|
|
|
|
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);
|
2025-03-17 00:32:06 +08:00
|
|
|
|
int8_t GO_SendData(UP_t *u,int id,float pos);
|
|
|
|
|
int8_t UP_angle(UP_t *u, fp32 target_angle) ;
|
2025-03-12 10:46:02 +08:00
|
|
|
|
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
|