标题: STM32倒立摆程序源码 [打印本页]
作者: 17393161229    时间: 2019-9-9 18:51
标题: STM32倒立摆程序源码
这个是已经调好的倒立摆的程序、有需要的赶紧下载哦！

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

  7. /**************************************************************************
  8. 函数功能：5MS定时中断函数 5MS控制周期
  9. 入口参数：无
  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);                                                 //===扫描按键状态
  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. 函数功能：直立PID控制
  36. 入口参数：角度、角速度
  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. 函数功能：速度PI控制
  52. 入口参数：左轮编码器、右轮编码器
  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;                     //===接收遥控器数据，控制前进后退
  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. 函数功能：赋值给PWM寄存器
  80. 入口参数：左轮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. 函数功能：读取编码器的数据并进行数据类型转换
  95. 入口参数：无
  96. 返回  值：无
  97. 作    者：Mini Balance
  98. **************************************************************************/
  99. void readEncoder(void)
  100. {
  101.           u16 Encoder_L;       //===左右编码器的脉冲计数       
  102.           Encoder_L= TIM3 -> CNT;        //===获取正交解码2数据       
  103.           TIM3 -> CNT=0;                       //===计数器清零
  104.                 if(Encoder_L>32768)  Encoder_Left=Encoder_L-65000; else  Encoder_Left=Encoder_L;  //===数据类型转换
  105. }

  107. /**************************************************************************
  108. 函数功能：限制PWM赋值
  109. 入口参数：无
  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. 函数功能：异常关闭电机
  124. 入口参数：倾角和电压
  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, 下载次数: 35)



作者: jiangkeqin_sy    时间: 2019-9-10 10:23
有图片吗？
作者: 扑扑    时间: 2019-9-12 08:58
赞一个
作者: collapsar147    时间: 2019-9-12 15:44
有没有接线图？光有程序没原理图人家怎么看，简单说明一下也好啊。



欢迎光临 (http://www.51hei.com/bbs/) Powered by Discuz! X3.1