找回密碼
 立即注冊

QQ登錄

只需一步，快速開始

搜索
查看: 3501|回復(fù): 0
打印 上一主題 下一主題
收起左側(cè)

2015年電賽練習(xí)風(fēng)力擺代碼 STM32F4 + jy61姿態(tài)傳感器 四路pwm 四個空心杯電機 PID

[復(fù)制鏈接]
跳轉(zhuǎn)到指定樓層
樓主
ID:293035 發(fā)表于 2020-3-25 09:55 | 只看該作者 |只看大圖 回帖獎勵 |倒序瀏覽 |閱讀模式
2015年全國電子設(shè)計競賽練習(xí)風(fēng)力擺代碼  STM32F4 + jy61姿態(tài)傳感器 四路pwm控制 四個空心杯電機 PID控制風(fēng)力擺畫圓，直線等等

制作出來的實物圖如下：


單片機源程序如下:
  1. #include "sys.h"
  2. #include "delay.h"
  3. #include "usart.h"
  4. #include "led.h"
  5. #include "lcd.h"
  6. #include "remote.h"
  7. #include "pwm.h"
  8. #include "motor.h"
  9. #include "math.h"
  10. #include "timer.h"
  11. //void Get_Angle();//獲取角度
  12. //void Get_Accelerate();//獲取角度
  13. //void Get_Palstance();//獲取角度
  14. void LCD_display(void);
  15. extern u8 Re_buf[11],counter;
  16. extern u8 jiasudu[6],jiaosudu[6],jiaodu[6];
  17. extern float Angle[3],Accelerate[3],Palstance[3],Temperature;
  18. float Err_X,Err_Y,Err_X_Last,Err_Y_Last;
  19. float X,Y;
  20. float Aim_X,Aim_Y;
  21. float Kp,Ki,Kd;
  22. u32 pwm1 = 0,pwm2 = 0,pwm3 = 0,pwm4 = 0;

  24. int main(void)
  25. {
  26.         u8 key;
  27.         u8 *str=0;
  28.         Kp = 100;
  29.         NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);//設(shè)置系統(tǒng)中斷優(yōu)先級分組2
  30.         delay_init(168);  //初始化延時函數(shù)
  31.         uart_init(115200);                //初始化串口波特率為115200
  32.         USART2_Init(115200);  //串口2初始化
  33.         delay_ms(1000);
  34.        
  35.         TIM12_PWM_Init(10000-1,21-1);//PWM1
  36.         TIM13_PWM_Init(10000-1,21-1);//PWM2
  37.         TIM14_PWM_Init(10000,21-1);//PWM3
  38.         TIM5_PWM_Init(10000-1,21-1);//PWM4

  40.         TIM3_Init(200-1,8400-1);    //定時器3初始化
  41.         Motor_Init();  //四路空心杯電機初始化
  42.         //LED_Init();                                        //初始化LED
  43.         LCD_Init();
  44.         Remote_Init();                                //紅外接收初始化                        
  45.        
  46.         POINT_COLOR=RED;//設(shè)置字體為紅色             
  47.         LCD_ShowString(30,130,200,16,16,"PWM1:");       
  48.   LCD_ShowString(30,150,200,16,16,"PWM2:");       
  49.   LCD_ShowString(30,170,200,16,16,"PWM3:");                
  50.   LCD_ShowString(30,190,200,16,16,"PWM4:");               
  51.         LCD_ShowString(30,210,200,16,16,"X_Angle:");
  52.         LCD_ShowString(30,230,200,16,16,"Y_Angle:");
  53.         LCD_ShowString(30,250,200,16,16,"Z_Angle:");
  54.        
  55.         Motor_Stop();  //電機置1高電平
  56.        
  57.         //Motor_Stop1();
  58.        
  59.         while(1)
  60.         {       
  61.                 LCD_display();
  62.         }
  63. }


  66. void TIM3_IRQHandler(void)//20ms進(jìn)入一次中斷
  67. {
  68.     if(TIM_GetITStatus(TIM3,TIM_IT_Update)==SET) //溢出中斷
  69.                 {
  70.                
  71.                                 Aim_X = 12.0;
  72.                                 Aim_Y = 0;
  73.                                
  74.                                 X = Angle[0];
  75.                                 Y = Angle[1];
  76.                                
  77.                                 Err_X = X - Aim_X;
  78.                                 Err_Y = Y - Aim_Y;
  79.                                
  80.                                
  81.                                 if(Err_X <= 0)
  82.                                 {
  83.                                          Motor_Drive(2,1); //電機2正轉(zhuǎn)
  84.                                          Motor_Drive(4,0); //電機4反轉(zhuǎn)
  85.                                          pwm2 += Kp*(fabs(Err_X))*0.02;
  86.                                          pwm4 += Kp*(fabs(Err_X))*0.02;
  87.                                 }
  88.                                 else
  89.                                 {
  90.                                          Motor_Drive(2,1);  //電機2反轉(zhuǎn)
  91.                                          Motor_Drive(4,0);  //電機4正轉(zhuǎn)
  92.                                          pwm2 -= Kp*Err_X*0.02;
  93.                                          pwm4 -= Kp*Err_X*0.02;
  94.                                 }
  95.                                
  96.                                 if(Err_Y <= 0)
  97.                                 {
  98.                                          Motor_Drive(1,1); //電機1正轉(zhuǎn)
  99.                                          Motor_Drive(3,0); //電機3反轉(zhuǎn)
  100.                                          pwm1 += Kp*(fabs(Err_Y))*0.02;
  101.                                          pwm3 += Kp*(fabs(Err_Y))*0.02;
  102.                                 }
  103.                                 else
  104.                                 {
  105.                                          Motor_Drive(1,0);  //電機1反轉(zhuǎn)
  106.                                          Motor_Drive(3,1);  //電機3正轉(zhuǎn)
  107.                                          pwm1 += Kp*Err_Y*0.02;
  108.                                          pwm3 += Kp*Err_Y*0.02;
  109.                                 }
  110.                                
  111.                                 //限幅
  112.                                 if(pwm1 >= 9000) pwm1 = 9000;
  113.                                 if(pwm2 >= 9000) pwm2 = 9000;
  114.                                 if(pwm3 >= 9000) pwm3 = 9000;
  115.                                 if(pwm4 >= 9000) pwm4 = 9000;
  116.                                
  117.                                 TIM_SetCompare1(TIM13,pwm1);
  118.                                 TIM_SetCompare1(TIM14,pwm2);
  119.                                 TIM_SetCompare1(TIM12,pwm3);
  120.                                 TIM_SetCompare1(TIM5,pwm4);
  121.                        
  122.                 }
  123.                 TIM_ClearITPendingBit(TIM3,TIM_IT_Update);  //清除中斷標(biāo)志位
  124. }



  128. void LCD_display()
  129. {
  130.        
  131.          LCD_ShowNum(86,130,pwm1,5,16);
  132.          LCD_ShowNum(86,150,pwm2,5,16);
  133.          LCD_ShowNum(86,170,pwm3,5,16);
  134.          LCD_ShowNum(86,190,pwm4,5,16);
  135.        
  136.    if(Angle[0] >= 0)
  137.                  {
  138.                          LCD_ShowChar(128,210,' ',16,0);
  139.                          LCD_Showfloat(144,210,Angle[0],4,16);               
  140.                  }
  141.                  else
  142.                  {
  143.                     LCD_ShowChar(128,210,'-',16,0);
  144.                           LCD_Showfloat(144,210,fabs(Angle[0]),4,16);               
  145.                  }
  146.                  
  147.                  if(Angle[1] >= 0)
  148.                  {
  149.                          LCD_ShowString(128,230,200,16,16," ");
  150.                          LCD_Showfloat(144,230,Angle[1],4,16);               
  151.                  }
  152.                  else
  153.                  {
  154.                     LCD_ShowString(128,230,200,16,16,"-");
  155.                           LCD_Showfloat(144,230,fabs(Angle[1]),4,16);               
  156.                  }
  157.                  
  158.                  if(Angle[2] >= 0)
  159.                  {
  160.                          LCD_ShowString(128,250,200,16,16," ");
  161.                    LCD_Showfloat(144,250,Angle[2],4,16);                //顯示鍵值                       
  162.                  }
  163.                  else
  164.                  {
  165.                     LCD_ShowString(128,250,200,16,16,"-");
  166.                           LCD_Showfloat(144,250,fabs(Angle[2]),4,16);                //顯示鍵值               
  167.                  }
  168.                  LCD_Fill(86,170,116+8*8,170+16,WHITE);        //清楚之前的顯示
  169.                  
  170. }

  172. /*
  173. void Get_Angle()//獲取角度
  174. {
  175.    Angle[0] = ((short)((jiaodu[1]<<8)|jiaodu[0]))/32768.0*180;//x軸角度
  176.    Angle[1] = ((short)((jiaodu[3]<<8)|jiaodu[2]))/32768.0*180;//Y軸角度
  177.          Angle[2] = ((short)((jiaodu[5]<<8)|jiaodu[4]))/32768.0*180;//Z軸角度
  178.          printf("x軸角度:%.3f,Y軸角度:%.3f,Z軸角度:%.3f\r\n",Angle[0],Angle[1],Angle[2]);
  179.          //delay_ms(1);
  180. }

  182. void Get_Accelerate()//獲取加速度
  183. {
  184.    Accelerate[0] = ((short)((jiasudu[1]<<8)|jiasudu[0]))/32768.0*16;//X軸加速度
  185.          Accelerate[1] = ((short)((jiasudu[3]<<8)|jiasudu[2]))/32768.0*16;//Y軸加速度
  186.          Accelerate[2] = ((short)((jiasudu[5]<<8)|jiasudu[4]))/32768.0*16;//Z軸加速度
  187.          printf("x軸加速度:%.3f,Y軸加速度:%.3f,Z軸加速度:%.3f\r\n",Accelerate[0],Accelerate[1],Accelerate[2]);
  188.          delay_ms(10);
  189. }
  190. void Get_Palstance()//獲取角速度
  191. {
  192.    Palstance[0] = ((short)((jiaosudu[1]<<8)|jiaosudu[0]))/32768.0*2000;//X軸角速度
  193.          Palstance[1] = ((short)((jiaosudu[3]<<8)|jiaosudu[2]))/32768.0*2000;//Y軸角速度
  194.          Palstance[2] = ((short)((jiaosudu[5]<<8)|jiaosudu[4]))/32768.0*2000;//Z軸角速度
  195.          printf("x軸角速度:%.2f,Y軸角速度:%.2f,Z軸角速度:%.2f\r\n",Palstance[0],Palstance[1],Palstance[2]);
  196.          delay_ms(10);
  197. }

  199. */

  201.         /*
  202.     key=Remote_Scan();       
  203.                 if(key)
  204.                 {                
  205.       LCD_ShowNum(86,130,pwm1,5,16);                //顯示鍵值                       
  206.                         LCD_ShowNum(86,150,pwm2,5,16);                //顯示鍵值                       
  207.                         LCD_ShowNum(86,170,pwm3,5,16);                //顯示鍵值                       
  208.                         LCD_ShowNum(86,190,pwm4,5,16);                //顯示鍵值                       
  209.                         switch(key)
  210.                         {
  211.                                
  212. ……………………

  214. …………限于本文篇幅 余下代碼請從51黑下載附件…………
復(fù)制代碼

所有資料51hei提供下載:
風(fēng)力擺.7z (346.31 KB, 下載次數(shù): 94)

評分

參與人數(shù) 1黑幣 +50 收起 理由
admin + 50 共享資料的黑幣獎勵！

查看全部評分

分享到:  QQ好友和群QQ好友和群 QQ空間QQ空間 騰訊微博騰訊微博 騰訊朋友騰訊朋友
收藏收藏3 分享淘帖 頂頂1 踩踩
回復(fù)

使用道具 舉報

您需要登錄后才可以回帖 登錄 | 立即注冊

本版積分規(guī)則

手機版|小黑屋|51黑電子論壇 |51黑電子論壇6群 QQ 管理員QQ:125739409;技術(shù)交流QQ群281945664

Powered by 單片機教程網(wǎng)

快速回復(fù) 返回頂部 返回列表