资源描述
附录2:风力摆源程序
#include "sys.h"
#include "usart.h"
#include "delay.h"
#include "led.h"
#include "oled.h"
#include "key.h"
#include "mpu6050.h"
#include "usmart.h"
#include "inv_mpu.h"
#include "inv_mpu_dmp_motion_driver.h"
#include "pwm.h"
#include "math.h"
float setpoint,rx,c;
u16 zk1=20000,zk2=20000,zk3=20000,zk4=20000;
float pitch,roll,yaw; //?·à-??
short temp; //???è
float et,et2,etsum,det,output,etp,etp2,etsump,detp,outputp;
float kp,ki,kd;
void pid()
{
et=setpoint-roll;
if(et>0)
{etsum=etsum+et;
det=et-et2;
output=kp*et+ki*etsum+kd*det;
et2=et;}
if(et<=0)
{output=24900;}
et2=et;
}
void pid2() //ó?à′??DD?à?ypitch
{
etp=setpoint-pitch;
etp=fabs(etp);
if(etp>=2)
{etsump=etsump+etp;
detp=etp-etp2;
outputp=kp*etp+ki*etsump+kd*detp;}
if((et>=0)&&(et<2))
{outputp=24000;}
etp2=etp;
}
void pid4r()
{
et=setpoint-roll; //ó?à′??DD?à?y
et=fabs(et);
if(et>=2)
{etsum=etsum+et;
det=et-et2;
output=kp*et+ki*etsum+kd*det;}
if((et>=0)&&(et<2))
{output=24000;}
et2=et;
}
void pid4p() //ó?à′??DD?à?ypitch
{
etp=setpoint-pitch;
etp=fabs(etp);
if(etp>=2)
{etsump=etsump+etp;
detp=etp-etp2;
outputp=kp*etp+ki*etsump+kd*detp;}
if((et>=0)&&(et<2))
{outputp=24000;}
etp2=etp;
}
void pid5r()
{
et=setpoint-roll; //ó?à′??DD?à?y
if(et>0)
{etsum=etsum+et;
det=et-et2;
output=kp*et+ki*etsum+kd*det;
et2=et;}
if(et<=0)
{output=24900;}
et2=et;
}
void pid5p() //ó?à′??DD?à?ypitch
{
etp=setpoint-pitch;
if(etp>0)
{etsump=etsump+etp;
detp=etp-etp2;
outputp=kp*etp+ki*etsump+kd*detp;
etp2=etp;}
if(etp<=0)
{outputp=24900;}
etp2=etp;
}
void init_all()
{
NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);
delay_init(); //?óê±oˉêy3?ê??ˉ
uart_init(500000); //′??ú3?ê??ˉ?a9600
usmart_dev.init(72);
LED_Init(); //3?ê??ˉó?LEDá??óμ?ó2?t?ó?ú
KEY_Init(); //3?ê??ˉ°′?ü
OLED_Init(); //3?ê??ˉLCD
MPU_Init(); //3?ê??ˉMPU6050
mpu_dmp_init();
TIM3_OC1PWM_Init(59999,2);
TIM3_OC2PWM_Init(59999,2);
TIM3_OC3PWM_Init(59999,2);
TIM3_OC4PWM_Init(59999,2);
OLED_ShowString(8,0,"Wave By Wind");
OLED_Refresh_Gram();
while(mpu_dmp_init())
{ OLED_Clear();
OLED_ShowString(8,0,"6050 Error");
OLED_Refresh_Gram();
delay_ms(400);
}
OLED_Clear();
}
int main(void)
{
init_all();
while(1) //1|?ü?????£ê?
{
OLED_ShowString(8,0,"CHOOSE");
OLED_ShowString(8,16,"A B C D E F");
OLED_ShowString(8,32,"1 2 3 4 5 6");
OLED_Refresh_Gram();
if(!KEYA){delay_ms(100);if(!KEYA){goto function1;}}
if(!KEYB){delay_ms(100);if(!KEYB){goto function2;}}
if(!KEYC){delay_ms(100);if(!KEYC){goto function3;}}
if(!KEYD){delay_ms(100);if(!KEYD){goto function4;}}
if(!KEYE){delay_ms(100);if(!KEYE){goto function5;}}
if(!KEYF){delay_ms(100);if(!KEYF){goto function6;}}
}
function1:while(1)
{
}
function2:while(1)
{
rx=15;kp=8000,ki=2,kd=1;
OLED_Clear();
while(1)
{
OLED_ShowString(72, 0,"RX");
OLED_ShowString(72,16,"P");
OLED_ShowString(72,32,"I");
OLED_ShowString(72,48,"D");
OLED_Refresh_Gram();
if(!KEYA){delay_ms(100); if(!KEYA){kp+=1000;}}
if(!KEYB){delay_ms(100); if(!KEYB){kp-=1000;}}
if(!KEYC){delay_ms(50); if(!KEYC){ki+=1;}}
if(!KEYD){delay_ms(50); if(!KEYD){ki-=1;}}
if(!KEYE){delay_ms(100); if(!KEYC){kd+=1;}}
if(!KEYF){delay_ms(100); if(!KEYD){kd-=1;}}
if(!KEYG){delay_ms(100); if(!KEYG){rx++;}}
if(!KEYH){delay_ms(100); if(!KEYH){rx--;}}
c=atan(rx/79.0);
setpoint=c*180/3.14;
OLED_ShowNum(80,0,rx,5,16); //??ê???êy2?·?
OLED_ShowNum(80,16,kp,5,16); //??ê???êy2?·?
OLED_ShowNum(80,32,ki,5,16); //??ê?D?êy2?·?
OLED_ShowNum(80,48,kd,5,16); //??ê?D?êy2?·?
OLED_Refresh_Gram();
angeldisplay(); //??ê??·à-??
if(!KEY1){delay_ms(100);if(!KEY1){goto looprun1;}}
}
looprun1: while(1)
{
mpu_dmp_get_data(&pitch,&roll,&yaw); //μ?μ??·à-??
if(roll>0)
{
mpu_dmp_get_data(&pitch,&roll,&yaw);
pid();
zk4=output;
if(zk4>=31000){zk4=31000;}
if(zk4<=24900){zk4=24900;}
TIM_SetCompare4(TIM3,zk4); TIM_SetCompare2(TIM3,24000);
}
if(roll<0)
{
mpu_dmp_get_data(&pitch,&roll,&yaw);
roll=-roll;
pid();
zk2=output;
if(zk2>=31000){zk2=31000;}
if(zk2<=24900){zk2=24900;}
TIM_SetCompare2(TIM3,zk2); TIM_SetCompare4(TIM3,24000);
}
angeldisplay();//???è??ê?//?ˉ3é?·à-????è?
}
}
function3:while(1)
{
}
function4:while(1)
{
setpoint=0;kp=4000,ki=1,kd=1;
OLED_Clear();
while(1)
{
OLED_ShowString(72, 0,"s");
OLED_ShowString(72,16,"P");
OLED_ShowString(72,32,"I");
OLED_ShowString(72,48,"D");
if(!KEYA){delay_ms(100); if(!KEYA){kp+=100;}}
if(!KEYB){delay_ms(100); if(!KEYB){kp-=100;}}
if(!KEYC){delay_ms(50); if(!KEYC){ki+=1;}}
if(!KEYD){delay_ms(50); if(!KEYD){ki-=1;}}
if(!KEYE){delay_ms(100); if(!KEYC){kd+=1;}}
if(!KEYF){delay_ms(100); if(!KEYD){kd-=1;}}
if(!KEYG){delay_ms(100); if(!KEYG){setpoint++;}}
if(!KEYH){delay_ms(100); if(!KEYH){setpoint--;}}
OLED_ShowNum(80,0,setpoint,5,16); //??ê???êy2?·?
OLED_ShowNum(80,16,kp,5,16); //??ê???êy2?·?
OLED_ShowNum(80,32,ki,5,16); //??ê?D?êy2?·?
OLED_ShowNum(80,48,kd,5,16); //??ê?D?êy2?·?
OLED_Refresh_Gram();
angeldisplay(); //??ê??·à-??
if(!KEY1){delay_ms(100);if(!KEY1){goto looprun4;}}
}
looprun4: while(1)
{
while(1)
{
mpu_dmp_get_data(&pitch,&roll,&yaw); //μ?μ??·à-??
if((roll<=0)&&(pitch<0))
{
pid4r();
zk4=output;
pid4p();
zk3=outputp;
if(zk4>=27000){zk4=27000;}
if(zk4<=24000){zk4=24000;}
if(zk3>=27000){zk3=27000;}
if(zk3<=24000){zk3=24000;}
TIM_SetCompare4(TIM3,zk4); TIM_SetCompare3(TIM3,zk3); TIM_SetCompare2(TIM3,24000); TIM_SetCompare1(TIM3,24000);
}
if((roll<0)&&(pitch>0))
{
pid4r();
zk4=output;
pid4p();
zk1=outputp;
if(zk4>=27000){zk4=27000;}
if(zk4<=24000){zk4=24000;}
if(zk1>=27000){zk1=27000;}
if(zk1<=24000){zk1=24000;}
TIM_SetCompare4(TIM3,zk4); TIM_SetCompare1(TIM3,zk1); TIM_SetCompare3(TIM3,24000); TIM_SetCompare2(TIM3,24000);
}
if((roll>=0)&&(pitch>0))
{
pid4r();
zk2=output;
pid4p();
zk1=outputp;
if(zk2>=27000){zk2=27000;}
if(zk2<=24000){zk2=24000;}
if(zk1>=27000){zk1=27000;}
if(zk1<=24000){zk1=24000;}
TIM_SetCompare2(TIM3,zk2); TIM_SetCompare1(TIM3,zk1); TIM_SetCompare3(TIM3,24000); TIM_SetCompare4(TIM3,24000);
}
if((roll>0)&&(pitch<0))
{
pid4r();
zk2=output;
pid4p();
zk3=outputp;
if(zk2>=27000){zk2=27000;}
if(zk2<=24000){zk2=24000;}
if(zk3>=27000){zk3=27000;}
if(zk3<=24000){zk3=24000;}
TIM_SetCompare2(TIM3,zk2); TIM_SetCompare3(TIM3,zk3); TIM_SetCompare4(TIM3,24000); TIM_SetCompare1(TIM3,24000);
}
}
}
}
function5:while(1)
{
setpoint=10;
kp=6000,ki=2,kd=1;
OLED_Clear();
while(1)
{
OLED_ShowString(72, 0,"s");
OLED_ShowString(72,16,"P");
OLED_ShowString(72,32,"I");
OLED_ShowString(72,48,"D");
if(!KEYA){delay_ms(100); if(!KEYA){kp+=100;}}
if(!KEYB){delay_ms(100); if(!KEYB){kp-=100;}}
if(!KEYC){delay_ms(50); if(!KEYC){ki+=1;}}
if(!KEYD){delay_ms(50); if(!KEYD){ki-=1;}}
if(!KEYE){delay_ms(100); if(!KEYC){kd+=1;}}
if(!KEYF){delay_ms(100); if(!KEYD){kd-=1;}}
if(!KEYG){delay_ms(100); if(!KEYG){setpoint++;}}
if(!KEYH){delay_ms(100); if(!KEYH){setpoint--;}}
OLED_ShowNum(80,0,setpoint,5,16); //??ê???êy2?·?
OLED_ShowNum(80,16,kp,5,16); //??ê???êy2?·?
OLED_ShowNum(80,32,ki,5,16); //??ê?D?êy2?·?
OLED_ShowNum(80,48,kd,5,16); //??ê?D?êy2?·?
OLED_Refresh_Gram();
angeldisplay(); //??ê??·à-??
if(!KEY1){delay_ms(100);if(!KEY1){goto looprun5;}}
}
looprun5: while(1)
{
setpoint=10;
kp=6000,ki=2,kd=1;
while(1)
{
mpu_dmp_get_data(&pitch,&roll,&yaw); //μ?μ??·à-??
if((roll<=0)&&(pitch<0))
{
pid5r();
zk2=output;
pid5p();
zk1=outputp;
if(zk2>=27000){zk2=27000;}
if(zk2<=24000){zk2=24000;}
if(zk1>=27000){zk1=27000;}
if(zk1<=24000){zk1=24000;}
TIM_SetCompare2(TIM3,zk2); TIM_SetCompare1(TIM3,zk1); TIM_SetCompare3(TIM3,24000); TIM_SetCompare4(TIM3,24000);
}
if((roll<0)&&(pitch>0))
{
pid5r();
zk2=output;
pid5p();
zk3=outputp;
if(zk2>=27000){zk2=27000;}
if(zk2<=24000){zk2=24000;}
if(zk3>=27000){zk3=27000;}
if(zk3<=24000){zk3=24000;}
TIM_SetCompare2(TIM3,zk2); TIM_SetCompare3(TIM3,zk3); TIM_SetCompare1(TIM3,24000); TIM_SetCompare4(TIM3,24000);
}
if((roll>=0)&&(pitch>0))
{
pid5r();
zk4=output;
pid5p();
zk3=outputp;
if(zk4>=27000){zk4=27000;}
if(zk4<=24000){zk4=24000;}
if(zk3>=27000){zk3=27000;}
if(zk3<=24000){zk3=24000;}
TIM_SetCompare4(TIM3,zk4); TIM_SetCompare3(TIM3,zk3); TIM_SetCompare1(TIM3,24000); TIM_SetCompare2(TIM3,24000);
}
if((roll>0)&&(pitch<0))
{
pid5r();
zk4=output;
pid5p();
zk1=outputp;
if(zk4>=27000){zk4=27000;}
if(zk4<=24000){zk4=24000;}
if(zk1>=27000){zk1=27000;}
if(zk1<=24000){zk1=24000;}
TIM_SetCompare4(TIM3,zk4); TIM_SetCompare1(TIM3,zk1); TIM_SetCompare2(TIM3,24000); TIM_SetCompare3(TIM3,24000);
}
}
}
}
function6:while(1)
{
}
}
10
展开阅读全文