在调节四旋翼PID的过程中，发现调节平衡时有很大的延迟，只是在调节P的过程中，当比例系数设置小一点的时候，四旋翼完全没有平衡自调节的过程。
以下是主程序，望各位大神指导：
#include "stm32f10x.h"
#include "sys.h"
#include "point.h"
#include "timer.h"
#include "delay.h"
#include "math.h"
#include "usart.h"
#include "mpu6050.h"
float P_PR=1.280;
float I_PR=0;
float D_PR=0;
float P_Y=0;
float I_Y=0;
float D_Y=0;
float image_roll=0;
float image_pitch=0;
float image_yaw=0;
float err_now_roll=0;
float err_last_roll=0;
float err_before_roll=0;
float err_now_pitch=0;
float err_last_pitch=0;
float err_before_pitch=0;
float err_now_yaw=0;
float err_last_yaw=0;
float err_before_yaw=0;
float increment_angle_r=0;
float increment_angle_p=0;
float increment_angle_y=0;
float PID_R(float angel)
{
image_roll=angel;
err_now_roll=image_roll-Roll;
increment_angle_r=P_PR*(err_now_roll-err_last_roll)+I_PR*err_now_roll+D_PR*(err_now_roll-2*err_last_roll+err_before_roll);
Roll+=increment_angle_r;
err_before_roll=err_last_roll;
err_last_roll=err_now_roll;
return Roll;
}
float PID_P(float angel)
{
// Pitch=Pitch+3; //°2×°?ó2?
image_pitch=angel;
err_now_pitch=image_pitch-Pitch;
increment_angle_p=P_PR*(err_now_pitch-err_last_pitch)+I_PR*err_now_pitch+D_PR*(err_now_pitch-2*err_last_pitch+err_before_pitch);
Pitch+=increment_angle_p;
err_before_pitch=err_last_pitch;
err_last_pitch=err_now_pitch;
return Pitch;
}
float PID_Y(float angel)
{
image_yaw=angel;
err_now_yaw=image_yaw-Yaw;
increment_angle_y=P_Y*(err_now_yaw-err_last_yaw)+I_Y*err_now_yaw+D_Y*(err_now_yaw-2*err_last_yaw+err_before_yaw);
Yaw+=increment_angle_y;
err_before_yaw=err_last_yaw;
err_last_yaw=err_now_yaw;
return Yaw;
} //pid
float pid_m1=0,pid_m2=0,pid_m3=0,pid_m4=0;
void Tim2_Init(void); //???2?????,????pwm
void TIM1_Int_Init(u16 arr,u16 psc);
void TIM3_Cap_Init(u16 arr,u16 psc);
extern u8 TIM4CH1_CAPTURE_STA; //ê?è?2???×′ì?
extern u16 TIM4CH1_CAPTURE_VAL; //ê?è?2????μ
float m1=0,m2=0,m3=0,m4=0,c=0,x=0; //pwm?????
u32 D=0,D1=0,RD=0,s=0;
int main(void)
{
delay_init();
uart_init(115200);
delay_ms(1000);
MPU6050_Init();
SystemInit(); //?????
POINT_Init();
Tim2_Init(); //??????2
TIM1_Int_Init(100,7199);
NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2); //éè??NVIC?D??·?×é2:2???à??ó??è??￡?2???ìó|ó??è??
TIM3_Cap_Init(0XFFFF,72-1); //ò?1Mhzμ??μ?ê??êy
while(1)
{
MPU6050_Pose();
printf("%f %f %f\n",Roll,Pitch,Yaw);
if(TIM4CH1_CAPTURE_STA&0X80)//3é1|2???μ?á?ò?′?é?éy??
{
D=TIM4CH1_CAPTURE_STA&0X3F;
D*=65536;//ò?3?ê±??×üoí
D+=TIM4CH1_CAPTURE_VAL;//μ?μ?×üμ???μ???ê±??
TIM4CH1_CAPTURE_STA=0;//?a????ò?′?2???
D1=0.0005*D*340;
if(RD>D1&&RD-D1<2000)
{
RD=D1;
}
else if(RD<D1&&D1-RD<2000)
{
RD=D1;
}
printf("%d\n",RD);
}
PID_R(0);
PID_P(0);
PID_Y(0);
m1=1000;
m3=1000;
pid_m1=m1+Pitch;
pid_m2=m2+Roll-Yaw;
pid_m3=m3-Pitch;
pid_m4=m4-Roll-Yaw;
TIM_SetCompare1(TIM2,pid_m1);
TIM_SetCompare2(TIM2,0);
TIM_SetCompare3(TIM2,pid_m3);
TIM_SetCompare4(TIM2,0); /////?è?¨
__NOP();
// if(s<80&&RD<300)
// {
// m1+=20;
// m2+=20;
// m3+=20;
// m4+=20;
// }
// else if(s<80&&RD>350)
// {
// m1-=50;
// m2-=50;
// m3-=50;
// m4-=50;
// }
// if(s>80)
// {
// m1-=50;
// m2-=50;
// m3-=50;
// m4-=50;
// }
// delay_ms(200);
// s++;
}
}
|
|
楼主
