110 lines
2.6 KiB
C
110 lines
2.6 KiB
C
/*
|
||
* 伸缩模组
|
||
*/
|
||
|
||
#pragma once
|
||
|
||
#ifdef __cplusplus
|
||
extern "C" {
|
||
#endif
|
||
|
||
/* Includes ----------------------------------------------------------------- */
|
||
#include "component/ahrs.h"
|
||
#include "component/filter.h"
|
||
#include "component/pid.h"
|
||
#include "device/motor.h"
|
||
#include "device/motor_dm.h"
|
||
#include "device/motor_rm.h"
|
||
/* Exported constants ------------------------------------------------------- */
|
||
#define TELESCOPING_OK (0) /* 运行正常 */
|
||
#define TELESCOPING_ERR (-1) /* 运行时发现了其他错误 */
|
||
#define TELESCOPING_ERR_NULL (-2) /* 运行时发现NULL指针 */
|
||
#define TELESCOPING_ERR_MODE (-3) /* 运行时配置了错误的Telescoping_CMD_t */
|
||
/* Exported macro ----------------------------------------------------------- */
|
||
/* Exported types ----------------------------------------------------------- */
|
||
typedef enum {
|
||
TELESCOPING_MODE_RELAX, /* 放松模式，电机不输出。 */
|
||
TELESCOPING_MODE_CLOCKWISE, /*顺时针模式 */
|
||
TELESCOPING_MODE_ANTI_CLOCKWISE, /*逆时针模式 */
|
||
} Telescoping_Mode_t;
|
||
|
||
typedef struct {
|
||
Telescoping_Mode_t mode;
|
||
float angle;
|
||
} Telescoping_CMD_t;
|
||
|
||
/* 云台参数的结构体，包含所有初始化用的参数，通常是const，存好几组。*/
|
||
typedef struct {
|
||
|
||
float circle;
|
||
|
||
MOTOR_RM_Param_t telescoping_motor;
|
||
struct {
|
||
KPID_Params_t Telescoping_velocity;
|
||
KPID_Params_t Telescoping_angle;
|
||
} pid;
|
||
|
||
/* 低通滤波器截止频率 */
|
||
struct {
|
||
float out; /* 电机输出 */
|
||
} low_pass_cutoff_freq;
|
||
|
||
} Telescoping_Params_t;
|
||
|
||
|
||
|
||
typedef struct {
|
||
struct {
|
||
MOTOR_Feedback_t telescoping; /*电机反馈 */
|
||
float gearbox_total_angle;
|
||
} motor;
|
||
} Telescoping_Feedback_t;
|
||
|
||
typedef struct {
|
||
float telescoping;
|
||
|
||
} Telescoping_Output_t;
|
||
|
||
typedef struct {
|
||
uint64_t lask_wakeup;
|
||
float dt;
|
||
|
||
|
||
Telescoping_Params_t *param; /* Init设定 */
|
||
|
||
/* 模块通用 */
|
||
Telescoping_Mode_t mode; /* 模式 */
|
||
|
||
/* PID计算的目标值 */
|
||
struct {
|
||
float telescoping;
|
||
} setpoint;
|
||
|
||
struct {
|
||
KPID_t telescoping_velocity; /* 电机速度环PID参数 */
|
||
KPID_t telescoping_angle; /* 电机位置环PID参数 */
|
||
} pid;
|
||
|
||
|
||
struct {
|
||
LowPassFilter2p_t telescoping;
|
||
} filter_out;
|
||
|
||
Telescoping_Output_t out; /* 云台输出 */
|
||
Telescoping_Feedback_t feedback; /* 反馈 */
|
||
|
||
} Telescoping_t;
|
||
|
||
int8_t Telescoping_Init(Telescoping_t *t,Telescoping_Params_t *param,
|
||
float target_freq);
|
||
|
||
int8_t Telescoping_UpdateFeedback(Telescoping_t *telescoping);
|
||
|
||
int8_t Telescoping_Control(Telescoping_t *t, Telescoping_CMD_t *t_cmd);
|
||
|
||
void Telescoping_Output(Telescoping_t *t);
|
||
|
||
#ifdef __cplusplus
|
||
}
|
||
#endif
|