大学生电子设计竞赛B题风力摆源程序.docx
《大学生电子设计竞赛B题风力摆源程序.docx》由会员分享，可在线阅读，更多相关《大学生电子设计竞赛B题风力摆源程序.docx（13页珍藏版）》请在冰豆网上搜索。
大学生电子设计竞赛B题风力摆源程序
附录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"
floatsetpoint,rx,c;
u16zk1=20000,zk2=20000,zk3=20000,zk4=20000;
floatpitch,roll,yaw;//?
·à-?
?
shorttemp;//è
floatet,et2,etsum,det,output,etp,etp2,etsump,detp,outputp;
floatkp,ki,kd;
voidpid（）
{
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;
}
voidpid2（）//ó?
à′?
?
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;
}
voidpid4r（）
{
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;
}
voidpid4p（）//ó?
à′?
?
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;
}
voidpid5r（）
{
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;
}
voidpid5p（）//ó?
à′?
?
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;
}
voidinit_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,"WaveByWind"）;
OLED_Refresh_Gram（）;
while（mpu_dmp_init（））
{OLED_Clear（）;
OLED_ShowString（8,0,"6050Error"）;
OLED_Refresh_Gram（）;
delay_ms（400）;
}
OLED_Clear（）;
}
intmain（void）
{
init_all（）;
while
（1）//1|?
ü￡ê?
{
OLED_ShowString（8,0,"CHOOSE"）;
OLED_ShowString（8,16,"ABCDEF"）;
OLED_ShowString（8,32,"123456"）;
OLED_Refresh_Gram（）;
if（!
KEYA）{delay_ms（100）;if（!
KEYA）{gotofunction1;}}
if（!
KEYB）{delay_ms（100）;if（!
KEYB）{gotofunction2;}}
if（!
KEYC）{delay_ms（100）;if（!
KEYC）{gotofunction3;}}
if（!
KEYD）{delay_ms（100）;if（!
KEYD）{gotofunction4;}}
if（!
KEYE）{delay_ms（100）;if（!
KEYE）{gotofunction5;}}
if（!
KEYF）{delay_ms（100）;if（!
KEYF）{gotofunction6;}}
}
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）{gotolooprun1;}}
}
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）{gotolooprun4;}}
}
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）{gotolooprun5;}}
}
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）
{
}
}
升级会员 