单片机论坛

标题: 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

  6. /**************************************************************************
  7. 函数功能5MS定时中断函数 5MS控制周期
  8. 入口参数无
  9. 返回  值无
  10. 作    者Mini Balance
  11. **************************************************************************/
  12. int zhongzhi=1750;
  13. int Balance_Pwm,Velocity_Pwm,Turn_Pwm;
  14. void TIM1_UP_TIM16_IRQHandler(void)  
  15. {   
  16.         if(TIM1->SR&0X0001)//5ms定时中断
  17.         {   
  18.                   TIM1->SR&=~(1<<0);                                       //===清除定时器1中断标志位                 
  19.                         readEncoder();                                           //===读取编码器的值
  20.                         adc=Get_Adc(0);
  21.                   Led_Flash(400);                                          //===LED闪烁;       
  22.                   Get_battery_volt();                                      //===获取电池电压                  
  23.                         key(100);                                                 //===扫描按键状态
  24.                   Balance_Pwm=balance(adc,zhongzhi);
  25.                   Velocity_Pwm=velocity(Encoder_Left);
  26.                   Moto1=Balance_Pwm-Velocity_Pwm;                 //===计算左值机最终PWM
  27.                    Xianfu_Pwm();                                            //===PWM限幅
  28.      if(Turn_Off(adc,Voltage)==0)                   //===如果不存在异常
  29.                         Set_Pwm(Moto1);                                    //===赋值给PWM寄存器                   
  30.         }      
  31. }

  32. /**************************************************************************
  33. 函数功能直立PID控制
  34. 入口参数角取角速度
  35. 返回  值直立控制PWM
  36. 作    者Mini Balance
  37. **************************************************************************/
  38. int balance(int adc, int target)
  39. {  
  40.          static int Last_Bias;
  41.          int balance,Bias;
  42.          Bias=adc-target;                                          //===求出平衡的角度中值 和机械相关
  43.          balance=15*Bias+50*(Bias-Last_Bias);                              //===计算平衡控制的电机PWM
  44.          Last_Bias=Bias;
  45.          return balance;
  46. }

  47. /**************************************************************************
  48. 函数功能速度PI控制
  49. 入口参数左轮编码器右轮编码器
  50. 返回  值速度控制PWM
  51. 作    者Mini Balance
  52. **************************************************************************/
  53. int velocity(int encoder_left)
  54. {  
  55.           static int Velocity,Encoder_Least,Encoder,Movement;
  56.           static long Encoder_Integral;
  57.           //=============遥控前进后退部分=======================//
  58.                 if(1==Flag_Qian)        Movement=-900;                     //===如果前进标志位置1 位移为负
  59.                 else if(1==Flag_Hou)          Movement=900;        //===如果后退标志位置1 位移为正
  60.           else  Movement=0;       
  61.    //=============速度PI控制器=======================//       
  62.                 Encoder_Least =Encoder_Left; //===获取最新速度偏差
  63.                 Encoder *= 0.4;                                         //===一阶低通滤波器      
  64.                 Encoder += Encoder_Least*0.6;                     //===一阶低通滤波器   
  65.                 Encoder_Integral +=Encoder;                                     //===积分出位移 积分时间5ms
  66.                 Encoder_Integral=Encoder_Integral-Movement;                     //===接收遥控器数据控制前进后退
  67.                 if(Encoder_Integral>500)          Encoder_Integral=500;          //===积分限幅
  68.                 if(Encoder_Integral<-500)        Encoder_Integral=-500;         //===积分限幅       
  69.                 Velocity=Encoder*300+Encoder_Integral*10; //===速度控制       
  70.                 if(Turn_Off(Angle_Balance,Voltage)==1)   Encoder_Integral=0;    //===电机关闭后清除积分
  71.           return Velocity;
  72. }


  73. /**************************************************************************
  74. 函数功能赋值给PWM寄存器
  75. 入口参数左轮PWM右轮PWM
  76. 返回  值无
  77. 作    者Mini Balance
  78. **************************************************************************/
  79. void Set_Pwm(int moto1)
  80. {
  81.                         PBout(3)=1;//===电机使能打开
  82.                         if(moto1<0)                        PBout(5)=1,                        PBout(4)=0;
  83.                         else                   PBout(5)=0,                        PBout(4)=1;
  84.                         TIM4->CCR2=myabs(moto1);

  85.        
  86. }
  87. /**************************************************************************
  88. 函数功能读取编码器的数据并进行数据类型转换
  89. 入口参数无
  90. 返回  值无
  91. 作    者Mini Balance
  92. **************************************************************************/
  93. void readEncoder(void)
  94. {
  95.           u16 Encoder_L;       //===左右编码器的脉冲计数       
  96.           Encoder_L= TIM3 -> CNT;        //===获取正交解码2数据       
  97.           TIM3 -> CNT=0;                       //===计数器清零
  98.                 if(Encoder_L>32768)  Encoder_Left=Encoder_L-65000; else  Encoder_Left=Encoder_L;  //===数据类型转换
  99. }

  100. /**************************************************************************
  101. 函数功能限制PWM赋值
  102. 入口参数无
  103. 返回  值无
  104. 作    者Mini Balance
  105. **************************************************************************/
  106. void Xianfu_Pwm(void)
  107. {       
  108.           int Amplitude=7100;    //===PWM满幅是7200 限制在7100
  109.     if(Moto1<-Amplitude) Moto1=-Amplitude;       
  110.                 if(Moto1>Amplitude)  Moto1=Amplitude;       
  111.           if(Moto2<-Amplitude) Moto2=-Amplitude;       
  112.                 if(Moto2>Amplitude)  Moto2=Amplitude;               
  113. }

  114. /**************************************************************************
  115. 函数功能异常关闭电机
  116. 入口参数倾角和电压
  117. 返回  值1异常  0正常
  118. 作    者Mini Balance
  119. **************************************************************************/
  120. u8 Turn_Off(int adc, int voltage)
  121. {
  122.             u8 temp;
  123.                         if(adc<(zhongzhi-500)||adc>(zhongzhi+500)||1==Flag_Stop||Voltage<1110)//===电压低于11.1V 关闭电机
  124.                         {                                                    
  125.       temp=1;                                          
  126.                         PBout(3)=0;
  127.                         PBout(12)=0;
  128.                         PAout(15)=0;
  129.                         PBout(4)=0;       
  130.                         PBout(5)=0;
  131.       }
  132.                         else
  133.       temp=0;
  134.       return temp;                       
  135. }
  136.        
复制代码

所有资料51hei提供下载:
倒立摆2.3.7z (354.95 KB, 下载次数: 7)




作者: jiangkeqin_sy    时间: 2019-9-10 10:23
有图片吗
作者: 扑扑    时间: 2019-9-12 08:58
赞一个

作者: collapsar147    时间: 2019-9-12 15:44
有没有接线图光有程序没原理图人家怎么看简单说明一下也好啊




欢迎光临 单片机论坛 (http://www.fzjdhmc.com/bbs/) Powered by Discuz! X3.1
ձһhƬձƵ ձƵɫwww ձƵ:ɫ