171 lines
4.8 KiB
C
171 lines
4.8 KiB
C
|
|
/*
|
|
* 伸缩云台模组
|
|
模式自行选择：遥控器改变或自行初始化
|
|
delta获取从遥控器自行获取
|
|
*/
|
|
|
|
/* Includes ----------------------------------------------------------------- */
|
|
#include "telescoping_gimal.h"
|
|
#include "gimbal.h"
|
|
#include "bsp/can.h"
|
|
#include "bsp/time.h"
|
|
#include "component/filter.h"
|
|
#include "component/pid.h"
|
|
#include "device/motor_rm.h"
|
|
#include "device/motor_dm.h"
|
|
/* Private typedef ---------------------------------------------------------- */
|
|
/* Private define ----------------------------------------------------------- */
|
|
/* Private macro ------------------------------------------------------------ */
|
|
/* Private variables -------------------------------------------------------- */
|
|
/* Private function -------------------------------------------------------- */
|
|
float motor_add_angle(float current_angle){
|
|
static int cirle;
|
|
static float prev_angle;
|
|
float delta = current_angle - prev_angle;
|
|
prev_angle=current_angle;
|
|
if(delta>M_PI){
|
|
cirle-=1;
|
|
}else if(delta<-M_PI){
|
|
cirle+=1;
|
|
}
|
|
current_angle+=cirle*M_2PI;
|
|
|
|
return current_angle;
|
|
}
|
|
|
|
|
|
/**
|
|
* \brief 设置伸缩模式
|
|
*
|
|
* \param c 包含云台数据的结构体
|
|
* \param mode 要设置的模式
|
|
*
|
|
* \return 函数运行结果
|
|
*/
|
|
static int8_t Telescoping_SetMode(Telescoping_t *t, Telescoping_Mode_t mode) {
|
|
if (t == NULL)
|
|
return -1;
|
|
if (mode == t->mode)
|
|
return TELESCOPING_OK;
|
|
|
|
PID_Reset(&t->pid.telescoping_angle);
|
|
PID_Reset(&t->pid.telescoping_velocity);
|
|
|
|
LowPassFilter2p_Reset(&t->filter_out.telescoping, 0.0f);
|
|
|
|
AHRS_ResetEulr(&(t->setpoint.telescoping)); /* 切换模式后重置设定值 */
|
|
|
|
return 0;
|
|
}
|
|
|
|
/* Exported functions ------------------------------------------------------- */
|
|
|
|
/**
|
|
* \brief 初始化伸缩电机
|
|
*
|
|
* \param g 包含伸缩电机数据的结构体
|
|
* \param param 包含伸缩电机参数的结构体指针
|
|
* \param target_freq 任务预期的运行频率
|
|
*
|
|
* \return 函数运行结果
|
|
*/
|
|
int8_t Telescoping_Init(Telescoping_t *t,Telescoping_Params_t *param,
|
|
float target_freq) {
|
|
if (t == NULL)
|
|
return -1;
|
|
t->param = param;
|
|
t->mode = TELESCOPING_MODE_CLOCKWISE; /* 设置默认模式 */
|
|
|
|
/* 初始化云台电机控制PID和LPF */
|
|
PID_Init(&(t->pid.telescoping_angle), KPID_MODE_CALC_D, target_freq,
|
|
&(t->param->pid.Telescoping_angle));
|
|
|
|
PID_Init(&(t->pid.telescoping_velocity), KPID_MODE_CALC_D, target_freq,
|
|
&(t->param->pid.Telescoping_velocity));
|
|
|
|
LowPassFilter2p_Init(&t->filter_out.telescoping, target_freq,
|
|
t->param->low_pass_cutoff_freq.out);
|
|
|
|
BSP_CAN_Init();
|
|
MOTOR_RM_Register(&(t->param->telescoping_motor));
|
|
|
|
return 0;
|
|
}
|
|
|
|
/**
|
|
* \brief 通过CAN设备更新云台反馈信息
|
|
*
|
|
* \param gimbal 云台
|
|
* \param can CAN设备
|
|
*
|
|
* \return 函数运行结果
|
|
*/
|
|
int8_t Telescoping_UpdateFeedback(Telescoping_t *telescoping) {
|
|
if (telescoping == NULL)
|
|
return -1;
|
|
|
|
MOTOR_RM_Update(&(telescoping->param->telescoping_motor));
|
|
MOTOR_RM_t *telescoping_motor_yaw = MOTOR_RM_GetMotor(&(telescoping->param->telescoping_motor));
|
|
telescoping->feedback.motor.telescoping = telescoping_motor_yaw->feedback;
|
|
telescoping->feedback.motor.gearbox_total_angle=motor_add_angle(telescoping->feedback.motor.telescoping.rotor_abs_angle);
|
|
return 0;
|
|
}
|
|
|
|
|
|
/**
|
|
|
|
*
|
|
* \param t 包含伸缩电机数据的结构体
|
|
* \param t_cmd 伸缩电机控制指令
|
|
*
|
|
* \return 函数运行结果
|
|
*/
|
|
|
|
int8_t Telescoping_Control(Telescoping_t *t, Telescoping_CMD_t *t_cmd) {
|
|
if (t == NULL || t_cmd == NULL) {
|
|
return -1;
|
|
}
|
|
t->dt = (BSP_TIME_Get_us() - t->lask_wakeup) / 1000000.0f;
|
|
t->lask_wakeup = BSP_TIME_Get_us();
|
|
// Telescoping_SetMode(t, t_cmd->mode);
|
|
// switch (t->mode){
|
|
// case TELESCOPING_MODE_RELAX:
|
|
// t->out.telescoping=0.0f;
|
|
// break;
|
|
// case TELESCOPING_MODE_CLOCKWISE:
|
|
// /*顺时针*/
|
|
// t->setpoint.telescoping=t->param->circle*M_2PI;
|
|
// break;
|
|
// case TELESCOPING_MODE_ANTI_CLOCKWISE:
|
|
// t->setpoint.telescoping=(-t->param->circle)*M_2PI;
|
|
// break;
|
|
// }
|
|
/* 控制相关逻辑 */
|
|
t->setpoint.telescoping=t->param->circle*M_2PI;
|
|
float telescoping_t_set_point;
|
|
|
|
telescoping_t_set_point = PID_Calc(&(t->pid.telescoping_angle), t->setpoint.telescoping,
|
|
t->feedback.motor.gearbox_total_angle, 0.0f, t->dt);
|
|
t->out.telescoping = PID_Calc(&(t->pid.telescoping_velocity), telescoping_t_set_point,
|
|
t->feedback.motor.telescoping.rotor_speed, 0.f, t->dt);
|
|
|
|
t->out.telescoping=(-t->out.telescoping);
|
|
|
|
/* 输出滤波 */
|
|
t->out.telescoping = LowPassFilter2p_Apply(&t->filter_out.telescoping, t->out.telescoping);
|
|
}
|
|
|
|
|
|
void Telescoping_Output(Telescoping_t *t){
|
|
|
|
MOTOR_RM_Ctrl(&t->param->telescoping_motor);
|
|
MOTOR_RM_SetOutput(&t->param->telescoping_motor, t->out.telescoping);
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|