设为首页收藏本站
开启辅助访问 切换到宽版

 找回密码
 立即注册

QQ登录

只需一步，快速开始

热门： 51单片机 | 24小时必答区 | 单片机教程 | 单片机DIY制作 | STM32 | Cortex M3 | 模数电子 | 电子DIY制作 | 音响/功放 | 拆机乐园 | Arduino | 嵌入式OS | 程序设计

搜索
»论坛 › 电子技术分类讨论区 › 资料共享 › stm32平衡车源码
返回列表 发新帖
查看: 2060|回复: 0
打印 上一主题 下一主题
收起左侧

stm32平衡车源码

[复制链接]
跳转到指定楼层
楼主
ID:402452 发表于 2018-11-25 20:06 | 只看该作者 回帖奖励 |倒序浏览 |阅读模式
单片机源程序如下:
  1. #include "sys.h"

  3. /**************************************************************************************************

  5. 程序作者：CUIT 3+1 Taocr


  8. ***************************************************************************************************/


  11. extern u16 Gyro_y, Gyro_z;
  12. u8 time_20ms, timer_5_flag, timer_10_flag;

  14. u8 Turn_Off(float angle)
  15. {
  16.             u8 temp;
  17.                         if(angle<-40||angle>40)
  18.                         {                                                         //===倾角大于40度关闭电机
  19.       temp=1;                                            //===Flag_Stop置1关闭电机
  20.                         PWMA2 = 0;
  21.                         PWMB2 = 0;
  22.                         PWMA1 = 0;
  23.                         PWMB1 = 0;
  24.       }
  25.                         else
  26.       temp=0;
  27.       return temp;                       
  28. }

  30. int velocity(int encoder_left,int encoder_right)
  31. {  
  32.     static float Velocity,Encoder_Least,Encoder,Movement;
  33.           static float Encoder_Integral;
  34.           float kp=300,ki=1.5;
  35.           //=============遥控前进后退部分=======================//
  36. //                if(1==Flag_Qian)        Movement=90/Flag_sudu;                     //===如果前进标志位置1 位移为负
  37. //                else if(1==Flag_Hou)          Movement=-90/Flag_sudu;        //===如果后退标志位置1 位移为正
  38. //          else  
  39.         Movement=0;       
  40.    //=============速度PI控制器=======================//       
  41.                 Encoder_Least =(encoder_left+encoder_right)-0;  //===获取最新速度偏差==测量速度（左右编码器之和）-目标速度（此处为零）
  42.                 Encoder *= 0.8;                                         //===一阶低通滤波器      
  43.                 Encoder += Encoder_Least*0.2;                     //===一阶低通滤波器   
  44.                 Encoder_Integral +=Encoder;                                  //===积分出位移 积分时间：10ms
  45.                 Encoder_Integral=Encoder_Integral-Movement;                  //===接收遥控器数据，控制前进后退
  46.                 if(Encoder_Integral>10000)          Encoder_Integral=10000;         //===积分限幅
  47.                 if(Encoder_Integral<-10000)        Encoder_Integral=-10000;         //===积分限幅       
  48.                 Velocity=Encoder*kp+Encoder_Integral*ki; //===速度控制       
  49.                 if(Turn_Off(Pitch)==1)   Encoder_Integral=0;    //===电机关闭后清除积分
  50.           return Velocity;
  51. }

  53. int turn(int encoder_left,int encoder_right,float gyro)//转向控制
  54. {
  55.           static float Turn_Target,Turn,Encoder_temp,Turn_Convert=0.9,Turn_Count;
  56.           float Turn_Amplitude=88/2,Kp=42,Kd=0;     
  57.           //=============遥控左右旋转部分=======================//
  58. //          if(1==Flag_Left||1==Flag_Right)                      //这一部分主要是根据旋转前的速度调整速度的起始速度，增加小车的适应性
  59. //                {
  60. //                        if(++Turn_Count==1)
  61. //                        Encoder_temp=myabs(encoder_left+encoder_right);
  62. //                        Turn_Convert=50/Encoder_temp;
  63. //                        if(Turn_Convert<0.6)Turn_Convert=0.6;
  64. //                        if(Turn_Convert>3)Turn_Convert=3;
  65. //                }       
  66. //          else
  67. //                {
  68.                         Turn_Convert=0.9;
  69.                         Turn_Count=0;
  70.                         Encoder_temp=0;
  71. //                }                       
  72. //                if(1==Flag_Left)                   Turn_Target-=Turn_Convert;
  73. //                else if(1==Flag_Right)             Turn_Target+=Turn_Convert;
  74. //                else
  75.                         Turn_Target=0;
  76.        
  77.     if(Turn_Target>Turn_Amplitude)  Turn_Target=Turn_Amplitude;    //===转向速度限幅
  78.           if(Turn_Target<-Turn_Amplitude) Turn_Target=-Turn_Amplitude;
  79. //                if(Flag_Qian==1||Flag_Hou==1)  Kd=1;        
  80. //                else
  81.                         Kd=0;   //转向的时候取消陀螺仪的纠正 有点模糊PID的思想
  82.           //=============转向PD控制器=======================//
  83.                   Turn=-Turn_Target*Kp -gyro*Kd;                 //===结合Z轴陀螺仪进行PD控制
  84.           return Turn;
  85. }


  88. int main(void)
  89. {       
  90.         int Speed_R, Speed_L;       
  91.          int Motol_PWM, Moto2_PWM;
  92.          int flag = 0;
  93.          u8 Right_Flag = 0;

  95.         delay_init();                                                                     //延时初始化
  96.         NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);// 设置中断优先级分组2
  97.         LED_Init();
  98.   LCD_init();//OLED初始化
  99.         LCD_clear();//显示屏清屏       
  100.         PWM_Init(7199,0);
  101.         Encoder_Init_TIM3();
  102.         Encoder_Init_TIM2();
  103.         uart_init(115200);                           //初始化串口1
  104.         IIC_Init();                     //模拟IIC初始化
  105.   MPU6050_initialize();           //=====MPU6050初始化       
  106.         DMP_Init();                     //初始化DMP
  107. //        NRF24L01_Init();            //初始化NRF24L01           
  108.          

  110. //while(NRF24L01_Check())        //检查NRF24L01是否在位.       
  111. //        {
  112. //                delay_ms(200);
  113. //                 delay_ms(200);
  114. //        }
  115. printf("NRF24L01Ready!");
  116.   Timer1_Init(29,7199);
  117.         while(1)
  118.         {               
  119.                 if(time_20ms == 10)
  120.                 {
  121.                         time_20ms = 0;
  122.                 printf("X轴倾角%f  Y轴倾角%d   转向角%d  \r\n",Pitch,Speed_R,Speed_L);//向上位机发送数据
  123.                         show_size8float4_2f(0,0,Roll);
  124.                         show_size8float4_2f(0,4,Right_Flag);
  125.                         time_20ms = 0;}
  126.                 if(timer_5_flag == 1)
  127.                 {
  128.                         timer_5_flag = 0;
  129.                         timer_10_flag = !timer_10_flag;
  130. //                        if(timer_10_flag == 0)
  131. //                        {
  132.                                 Read_DMP();
  133. //                        }
  134.                         Speed_R = Read_Encoder(2);
  135.                         Speed_L = Read_Encoder(3);
  136.                        
  137.                         Motol_PWM = 650*(Roll-5)+0.5*gyro[0]-velocity(Speed_L,Speed_R);//+turn(Speed_L,Speed_R,gyro[2]);
  138.                         Moto2_PWM = 200*(Pitch)+1.5*gyro[1];//-velocity(Speed_L,Speed_R);//-turn(Speed_L,Speed_R,gyro[2]);
  139.                         if(Roll>15&&flag<10000){Motol_PWM = 5000;flag++;}
  140.                         if(Roll<-15&&flag<10000){Motol_PWM = -5000;flag++;}
  141.                         if(Motol_PWM>=0)
  142.                         {
  143.                                 if(Right_Flag == 0)
  144.                                 Right_Flag = 1;
  145.                                 PWMA1 = Motol_PWM;
  146.                                 PWMB1 = Motol_PWM;
  147.                                 PWMA2 = 1;
  148.                                 PWMB2 = 1;
  149.                         }
  150.                         else
  151.                         {
  152.                                 if(Right_Flag == 0)
  153.                                 Right_Flag = 2;
  154.                                 PWMA2 = -Motol_PWM;
  155.                                 PWMB2 = -Motol_PWM;
  156.                                 PWMA1 = 1;
  157.                                 PWMB1 = 1;
  158.                         }
  159.                 }
  160.                 if(Turn_Off(Pitch)==1){}
  161.                                
  162.         }          
  163. }

  165. int TIM1_UP_IRQHandler(void)  
  166. {   
  167.         if(TIM1->SR&0X0001)//5ms定时中断
  168.         {   
  169.                   TIM1->SR&=~(1<<0);                                       //===清除定时器1中断标志位       
  170.                         time_20ms++;       
  171.                         timer_5_flag = 1;               
  172.         }
  173.         return 0;
  174. }
复制代码

所有资料51hei提供下载:
平衡车源码.rar (393.04 KB, 下载次数: 25)


分享到:  QQ好友和群QQ好友和群 QQ空间QQ空间 腾讯微博腾讯微博 腾讯朋友腾讯朋友
收藏收藏2 分享淘帖 顶顶 踩踩
回复

使用道具 举报

返回列表 发新帖
高级模式
B Color Image Link Quote Code Smilies
您需要登录后才可以回帖 登录 | 立即注册

本版积分规则

手机版|小黑屋|51黑电子论坛 |51黑电子论坛6群 QQ 管理员QQ:125739409;技术交流QQ群281945664

Powered by 单片机教程网

快速回复 返回顶部 返回列表