#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]; //位置环 //trigger //编码器数据 const motor_measure_t *trigger_motor_data;//3508电机数据 pid_type_def t_speed_pid;//2006速度环pid结构体 pid_type_def t_angle_pid;//2006位置环pid结构体 fp32 M2006_speed_PID[3]={5.0,0.3,0.0}; //P,I,D(速度环) fp32 M2006_angle_PID[3]={25.0,0.0,1.5}; //P,I,D(角度环) //速度环pid参数 fp32 M2006_PID[3]={4.9,0.01,0.0}; int16_t t_result; //速度环 //can最后发送的数据 float t_angleSet[1]; //位置环 void motor_init(void) { // motor_3508_data=get_motor_point(0); trigger_motor_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); PID_init(&t_speed_pid,PID_POSITION,M2006_speed_PID,500000,500000); PID_init(&t_angle_pid,PID_POSITION,M2006_angle_PID,500000,100000); } float trigger_angle = 0; void trigger_pos(fp32 angle) { int16_t delta[1]; delta[0]=PID_calc(&t_angle_pid,trigger_motor_data->total_angle,angle); t_result=PID_calc(&t_speed_pid,trigger_motor_data->speed_rpm,delta[0]); } 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]); }