56 lines
1.4 KiB
C
56 lines
1.4 KiB
C
|
#include "dji.h"
|
|||
|
|
#include "TopDefine.h"
|
|||
|
|
#include "remote_control.h"
|
|||
|
|
#include "calc_lib.h"
|
|||
|
|
|
|||
|
|
//可以在此完成电机pid初始化，以及电机的控制
|
|||
|
|
//然后再去task里发送电机控制指令
|
|||
|
|
//应该也可以在此发送电机指令
|
|||
|
|
extern RC_mess_t RC_mess;
|
|||
|
|
|
|||
|
|
//编码器数据
|
|||
|
|
const motor_measure_t *motor_3508_data;//3508电机数据
|
|||
|
|
|
|||
|
|
pid_type_def speed_pid;//3508速度环pid结构体
|
|||
|
|
pid_type_def angle_pid;//3508位置环pid结构体
|
|||
|
|
|
|||
|
|
//pid
|
|||
|
|
//fp32 M3508_speed_PID[3]={10.0,0.3,0.0}; //P,I,D(速度环)
|
|||
|
|
//fp32 M3508_angle_PID[3]={10.0,0.0,1.5}; //P,I,D(角度环)
|
|||
|
|
|
|||
|
|
fp32 M3508_speed_PID[3]={5.0,0.3,0.0}; //P,I,D(速度环)
|
|||
|
|
fp32 M3508_angle_PID[3]={25.0,0.0,1.5}; //P,I,D(角度环)
|
|||
|
|
//速度环pid参数
|
|||
|
|
fp32 M3508_PID[3]={4.9,0.01,0.0};
|
|||
|
|
|
|||
|
|
int16_t result; //速度环
|
|||
|
|
float angleSet[MOTOR_NUM]; //位置环
|
|||
|
|
|
|||
|
|
void motor_init(void)
|
|||
|
|
{
|
|||
|
|
motor_3508_data=get_motor_point(0);
|
|||
|
|
|
|||
|
|
// PID_init(&speed_pid,PID_POSITION,M3508_speed_PID,3000,1000);
|
|||
|
|
// PID_init(&angle_pid,PID_POSITION,M3508_angle_PID,7000,2000);
|
|||
|
|
|
|||
|
|
PID_init(&speed_pid,PID_POSITION,M3508_speed_PID,500000,500000);
|
|||
|
|
PID_init(&angle_pid,PID_POSITION,M3508_angle_PID,500000,100000);
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
void motor_speed(fp32 speed)
|
|||
|
|
{
|
|||
|
|
|
|||
|
|
result=PID_calc(&speed_pid,motor_3508_data->speed_rpm,speed);
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
void motor_pos(fp32 angle)
|
|||
|
|
{
|
|||
|
|
int16_t delta[1];
|
|||
|
|
|
|||
|
|
delta[0]=PID_calc(&angle_pid,motor_3508_data->total_angle,angle);
|
|||
|
|
result=PID_calc(&speed_pid,motor_3508_data->speed_rpm,delta[0]);
|
|||
|
|
|
|||
|
|
}
|