標題: STM32倒立擺程序源碼 [打印本頁]
作者: 17393161229    時間: 2019-9-9 18:51
標題: STM32倒立擺程序源碼
這個是已經(jīng)調(diào)好的倒立擺的程序、有需要的趕緊下載哦！

單片機源程序如下:
  1. #include "MiniBalance.h"
  2. #include "math.h"
  3. #include "led.h"
  4. #include "mpu6050.h"
  5. #define PI 3.14159265

  7. /**************************************************************************
  8. 函數(shù)功能：5MS定時中斷函數(shù) 5MS控制周期
  9. 入口參數(shù)：無
  10. 返回  值：無
  11. 作    者：Mini Balance
  12. **************************************************************************/
  13. int zhongzhi=1750;
  14. int Balance_Pwm,Velocity_Pwm,Turn_Pwm;
  15. void TIM1_UP_TIM16_IRQHandler(void)  
  16. {   
  17.         if(TIM1->SR&0X0001)//5ms定時中斷
  18.         {   
  19.                   TIM1->SR&=~(1<<0);                                       //===清除定時器1中斷標志位                 
  20.                         readEncoder();                                           //===讀取編碼器的值
  21.                         adc=Get_Adc(0);
  22.                   Led_Flash(400);                                          //===LED閃爍;       
  23.                   Get_battery_volt();                                      //===獲取電池電壓                  
  24.                         key(100);                                                 //===掃描按鍵狀態(tài)
  25.                   Balance_Pwm=balance(adc,zhongzhi);
  26.                   Velocity_Pwm=velocity(Encoder_Left);
  27.                   Moto1=Balance_Pwm-Velocity_Pwm;                 //===計算左輪電機最終PWM
  28.                    Xianfu_Pwm();                                            //===PWM限幅
  29.      if(Turn_Off(adc,Voltage)==0)                   //===如果不存在異常
  30.                         Set_Pwm(Moto1);                                    //===賦值給PWM寄存器                   
  31.         }      
  32. }

  34. /**************************************************************************
  35. 函數(shù)功能：直立PID控制
  36. 入口參數(shù)：角度、角速度
  37. 返回  值：直立控制PWM
  38. 作    者：Mini Balance
  39. **************************************************************************/
  40. int balance(int adc, int target)
  41. {  
  42.          static int Last_Bias;
  43.          int balance,Bias;
  44.          Bias=adc-target;                                          //===求出平衡的角度中值 和機械相關
  45.          balance=15*Bias+50*(Bias-Last_Bias);                              //===計算平衡控制的電機PWM
  46.          Last_Bias=Bias;
  47.          return balance;
  48. }

  50. /**************************************************************************
  51. 函數(shù)功能：速度PI控制
  52. 入口參數(shù)：左輪編碼器、右輪編碼器
  53. 返回  值：速度控制PWM
  54. 作    者：Mini Balance
  55. **************************************************************************/
  56. int velocity(int encoder_left)
  57. {  
  58.           static int Velocity,Encoder_Least,Encoder,Movement;
  59.           static long Encoder_Integral;
  60.           //=============遙控前進后退部分=======================//
  61.                 if(1==Flag_Qian)        Movement=-900;                     //===如果前進標志位置1 位移為負
  62.                 else if(1==Flag_Hou)          Movement=900;        //===如果后退標志位置1 位移為正
  63.           else  Movement=0;       
  64.    //=============速度PI控制器=======================//       
  65.                 Encoder_Least =Encoder_Left; //===獲取最新速度偏差
  66.                 Encoder *= 0.4;                                         //===一階低通濾波器      
  67.                 Encoder += Encoder_Least*0.6;                     //===一階低通濾波器   
  68.                 Encoder_Integral +=Encoder;                                     //===積分出位移 積分時間：5ms
  69.                 Encoder_Integral=Encoder_Integral-Movement;                     //===接收遙控器數(shù)據(jù)，控制前進后退
  70.                 if(Encoder_Integral>500)          Encoder_Integral=500;          //===積分限幅
  71.                 if(Encoder_Integral<-500)        Encoder_Integral=-500;         //===積分限幅       
  72.                 Velocity=Encoder*300+Encoder_Integral*10; //===速度控制       
  73.                 if(Turn_Off(Angle_Balance,Voltage)==1)   Encoder_Integral=0;    //===電機關閉后清除積分
  74.           return Velocity;
  75. }


  78. /**************************************************************************
  79. 函數(shù)功能：賦值給PWM寄存器
  80. 入口參數(shù)：左輪PWM、右輪PWM
  81. 返回  值：無
  82. 作    者：Mini Balance
  83. **************************************************************************/
  84. void Set_Pwm(int moto1)
  85. {
  86.                         PBout(3)=1;//===電機使能打開
  87.                         if(moto1<0)                        PBout(5)=1,                        PBout(4)=0;
  88.                         else                   PBout(5)=0,                        PBout(4)=1;
  89.                         TIM4->CCR2=myabs(moto1);

  91.        
  92. }
  93. /**************************************************************************
  94. 函數(shù)功能：讀取編碼器的數(shù)據(jù)并進行數(shù)據(jù)類型轉換
  95. 入口參數(shù)：無
  96. 返回  值：無
  97. 作    者：Mini Balance
  98. **************************************************************************/
  99. void readEncoder(void)
  100. {
  101.           u16 Encoder_L;       //===左右編碼器的脈沖計數(shù)       
  102.           Encoder_L= TIM3 -> CNT;        //===獲取正交解碼2數(shù)據(jù)       
  103.           TIM3 -> CNT=0;                       //===計數(shù)器清零
  104.                 if(Encoder_L>32768)  Encoder_Left=Encoder_L-65000; else  Encoder_Left=Encoder_L;  //===數(shù)據(jù)類型轉換
  105. }

  107. /**************************************************************************
  108. 函數(shù)功能：限制PWM賦值
  109. 入口參數(shù)：無
  110. 返回  值：無
  111. 作    者：Mini Balance
  112. **************************************************************************/
  113. void Xianfu_Pwm(void)
  114. {       
  115.           int Amplitude=7100;    //===PWM滿幅是7200 限制在7100
  116.     if(Moto1<-Amplitude) Moto1=-Amplitude;       
  117.                 if(Moto1>Amplitude)  Moto1=Amplitude;       
  118.           if(Moto2<-Amplitude) Moto2=-Amplitude;       
  119.                 if(Moto2>Amplitude)  Moto2=Amplitude;               
  120. }

  122. /**************************************************************************
  123. 函數(shù)功能：異常關閉電機
  124. 入口參數(shù)：傾角和電壓
  125. 返回  值：1：異常  0：正常
  126. 作    者：Mini Balance
  127. **************************************************************************/
  128. u8 Turn_Off(int adc, int voltage)
  129. {
  130.             u8 temp;
  131.                         if(adc<(zhongzhi-500)||adc>(zhongzhi+500)||1==Flag_Stop||Voltage<1110)//===電壓低于11.1V 關閉電機
  132.                         {                                                    
  133.       temp=1;                                          
  134.                         PBout(3)=0;
  135.                         PBout(12)=0;
  136.                         PAout(15)=0;
  137.                         PBout(4)=0;       
  138.                         PBout(5)=0;
  139.       }
  140.                         else
  141.       temp=0;
  142.       return temp;                       
  143. }
  144.        
復制代碼

所有資料51hei提供下載:
倒立擺2.3.7z (354.95 KB, 下載次數(shù): 35)



作者: jiangkeqin_sy    時間: 2019-9-10 10:23
有圖片嗎？
作者: 撲撲    時間: 2019-9-12 08:58
贊一個
作者: collapsar147    時間: 2019-9-12 15:44
有沒有接線圖？光有程序沒原理圖人家怎么看，簡單說明一下也好啊。



歡迎光臨 (http://www.torrancerestoration.com/bbs/) Powered by Discuz! X3.1