收藏 分销(赏)

2015年全国大学生电子设计竞赛B题风力摆源程序.doc

上传人:仙人****88 文档编号:11980750 上传时间:2025-08-25 格式:DOC 页数:10 大小:60.04KB 下载积分:10 金币
下载 相关
2015年全国大学生电子设计竞赛B题风力摆源程序.doc_第1页
第1页 / 共10页
2015年全国大学生电子设计竞赛B题风力摆源程序.doc_第2页
第2页 / 共10页


点击查看更多>>
资源描述
附录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
展开阅读全文

开通  VIP、SVIP  下载更划算
下载10份以上建议开通 VIP 会员
下载20份以上建议开通SVIP会员


开通VIP      成为共赢上传

当前位置:首页 > 教育专区 > 其他

移动网页_全站_页脚广告1

关于我们      便捷服务       自信AI       AI导航        关注我们

©2010-2026 宁波自信网络信息技术有限公司  版权所有

客服电话:0574-28810668  投诉电话:18658249818

gongan.png浙公网安备33021202000488号   

icp.png浙ICP备2021020529号-1  |  浙B2-20240490  

关注我们 :微信公众号    抖音    微博    LOFTER 

客服