暑期答辩——基于STM32的自平衡小车

1、感想：

　　　　我想：我的一系列痛苦源泉都从这里出发，我为什么要做这个车，电路板你看你焊的是个啥子玩意，最后还不是用的是开发板下的程序，铁混子一个，好的，还是来看下我的开发经历和过程吧！

泼个图：

还请自动忽视掉这双好看的手与可爱的机器猫

2、元器件：

1、硬件资源：

带霍尔编码器的直流电机两个（偏心轮和带减速器），支架若干，杜邦线若干，L298N，降压模块JM34RPLM2596S，stm32核心板：（当时准备c8t6，但看了它原理图后去发现好像定时器只有一个这就让我突然懵逼，后来就改成了vct6的大容量的核心版，但后来的我其实发现STM32的板子其实所有功能差不多，只有flash的容量上的差别，潘总说得，说得我当场死去，没事）

MPU6050，12v航模电池。

2、软件资源：

定时器3个（定时器1用两个通道产生10KHZ的PWM波，定时器2，4设置为编码器模式，通过编码器得到两个轮子的转速，转速转换可见下面代码，定时器3设置为输入捕获模式用于超声波避障），串口通信两个，分别是USART1和USART3主要考虑引脚才用这两个串口，USART1用于接收数据到电脑上位机上显示波形，波特率为128000，用来调节PID参数，（一定要注意USART1he题目都是高级的，都挂接在APB2总线上）USART3用来与蓝牙连接，外部中断EXTI0，用于MPU6050 INT0产生下降沿后触发，用于控制电机的速度控制，模拟IIC（用于处理和接收MPU6050数据）

　　

void Encoder_Init_TIM2(void)
{
    TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;  
  TIM_ICInitTypeDef TIM_ICInitStructure;  
  GPIO_InitTypeDef GPIO_InitStructure;
  RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, ENABLE);
  RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE);
    
  GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0|GPIO_Pin_1;   
  GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING; 
  GPIO_Init(GPIOA, &GPIO_InitStructure);                         
  
  TIM_TimeBaseStructInit(&TIM_TimeBaseStructure);
  TIM_TimeBaseStructure.TIM_Prescaler = 0x0; 
  TIM_TimeBaseStructure.TIM_Period = ENCODER_TIM_PERIOD; 
  TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1;
  TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
  TIM_EncoderInterfaceConfig(TIM2, TIM_EncoderMode_TI12, TIM_ICPolarity_Rising, TIM_ICPolarity_Rising);
  TIM_ICStructInit(&TIM_ICInitStructure);
  TIM_ICInitStructure.TIM_ICFilter = 10;
  TIM_ICInit(TIM2, &TIM_ICInitStructure);
  TIM_ClearFlag(TIM2, TIM_FLAG_Update);
  TIM_ITConfig(TIM2, TIM_IT_Update, ENABLE);
  //Reset counter
  TIM_SetCounter(TIM2,0);
  TIM_Cmd(TIM2, ENABLE); 
}

void Encoder_Init_TIM4(void)
{
    TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;  
  TIM_ICInitTypeDef TIM_ICInitStructure;  
  GPIO_InitTypeDef GPIO_InitStructure;
  RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4, ENABLE);
  RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE);
    
  GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6|GPIO_Pin_7;    
  GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING; 
  GPIO_Init(GPIOB, &GPIO_InitStructure);                       
  
  TIM_TimeBaseStructInit(&TIM_TimeBaseStructure);
  TIM_TimeBaseStructure.TIM_Prescaler = 0x0; 
  TIM_TimeBaseStructure.TIM_Period = ENCODER_TIM_PERIOD; 
  TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1;
  TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
  TIM_TimeBaseInit(TIM4, &TIM_TimeBaseStructure);
  TIM_EncoderInterfaceConfig(TIM4, TIM_EncoderMode_TI12, TIM_ICPolarity_Rising, TIM_ICPolarity_Rising);
  TIM_ICStructInit(&TIM_ICInitStructure);
  TIM_ICInitStructure.TIM_ICFilter = 10;
  TIM_ICInit(TIM4, &TIM_ICInitStructure);
  TIM_ClearFlag(TIM4, TIM_FLAG_Update);
  TIM_ITConfig(TIM4, TIM_IT_Update, ENABLE);
  //Reset counter
  TIM_SetCounter(TIM4,0);
  TIM_Cmd(TIM4, ENABLE); 
}


int Read_Encoder(u8 TIMX)
{
    int Encoder_TIM;    
   switch(TIMX)
     {
       case 2:  Encoder_TIM= (short)TIM2 -> CNT;  TIM2 -> CNT=0;break;
         case 3:  Encoder_TIM= (short)TIM3 -> CNT;  TIM3 -> CNT=0;break;    
         case 4:  Encoder_TIM= (short)TIM4 -> CNT;  TIM4 -> CNT=0;break;    
         default:  Encoder_TIM=0;
     }
        return Encoder_TIM;
}

3、主要算法 PID，DMP，滤波

　　1、什么是PID

　　　　PID应该都知道是比例积分微分吧，直接来干货吧！

比如：让一个直木棒在手指尖上保持直立。这需要两个条件：一个是托着木棒的手掌可以移动；另一个是眼睛可以观察到木棒的倾斜角度和倾斜趋势（角速度）。通过手掌移动抵消木棒的倾斜角度和趋势，从而保持木棒的直立。这两个条件缺一不可，

                            

此时MPU6050传过来的位置数据就是我们的眼睛所采集的数据，而处理信息的大脑就变成了我们的单片机，大脑下达命令给手脚去做相应的移动，我们的单片机也通过产生时序不一的高低电平信号下达相应的指令去控制电机的转动，那怎么去下达这样的一个命令，下达一个怎样的命令才能符合我们对电机的控制，这就是我们要解决的核心问题

所以车模平衡控制也是通过负反馈来实现的，与上面保持木棒直立比较则相对简单。因为车模有两个轮子着地，车体只会在轮子滚动的方向上发生倾斜。控制轮子转动，抵消在一个维度上倾斜的趋势便可以保持车体平衡了

 

至于为什么，还请参考相关资料，我下次在更，

 

2、如何调节PID参数

我以前总认为ＰＩＤ参数，让它工作应该不难，难的是想让它工作的好一点，更加好一点。但现在看来很是错，对于象我这样的初学者往往只重视这个ＰＩＤ，却很是忽略ＰＩＤ能工作的“基础环境”。
　１、所谓的“基础环境”就是控制对象的工作范围，我就说说我自己在搞的这个电流环吧，比如说电流的基准值是０－５００，也就是说我的ＰＷＭ波的周期为１０００，哦，忘了说了，我控制方式是双极性Ｈ桥控制。这样在０－５００为正转，５００－１０００为反转，因为刚开始也没考虑那么多，结果去检测电流后的ＡＤ转换是０－１０２４，更要命的是我以为ＰＩＤ是万能的，只要把这些参数整一下也应该能搞定的，结果可想而知了，感叹自己的无知啊！！保险丝断了一大箩，这才恍然大悟，“万能的”ＰＩＤ参数给了我重重的打击。各个参数的工作范围设计是多么的重要啊。所谓的ＰＩＤ参数只是在这个基础环境之上工作与控制的。。。。
　２、现在我开始想搞速度控制了（速度环），结果你想好了，速度环的输出控制目标是电流环的给定电流参考值，但我搞的速度是０－３０００转速，现在不知道是怎么样把转速和这个电流参考输出值对应起来，就是说我的工作参数又不在一个范围了，转速是０－３０００，而电流值是０－５００，该怎么控制对应啊，我现在都晕了。。。。　望大家给点建议和思路，谢谢！！！

 

1、经验数据法

PID控制器的参数整定在大量的工程实践中，逐渐被广大工程技术人员经过大量的经验积累找到了一种快捷的整定方法，就是我们现在介绍的所谓“经验法”。实际上比例、积分和微分三部分作用是相互影响的，应用经验法可避免一些重复工作，节省调试时间，尤其是在缺少一些资料和试验数据的时候。从应用的角度看，只要被控对象主要指标达到设计要求，能满足现场要求即可。长期的实践经验发现，各种不同被控对象的PID的参数都是有一定规律的，也就是说有一定的数据范围。这样就为现场调试提供了一个大致基准，可方便依据此基准迅速查找。

2、试凑法

顾名思义，试凑法就是根据过渡过程中被调参数变化的情况进行再调整PID参数的方法。此法边观察过程曲线（过程变量变化情况），边修改参数，直到满意为止。

大家都知道，增大比例系数Kp会加快系统的响应速度，提高系统的快速性。但过大的比例系数会使系统有较大的超调，有可能产生振荡使稳定性变差，并且有稳态误差。减小积分系数KI将减少积分作用（与积分时间常数的变化相反），有利于减少超调使系统稳定，减小系统稳态误差，但系统消除静差的速度慢。增加微分系数KD有利于加快系统的响应，使系统提前作出响应，使超调减少，稳定性增加，但缺点明显，对干扰的抑制能力差，而且整定不当反而使系统处于不稳定状态。试凑时，一般可根据以上各参数特点，对参数实行先比例、后积分、再微分的步骤进行整定。

（1） 比例部分整定。首先将积分系数KI和微分系数KD置零，取消微分和积分作用而采用纯比例控制。将比例系数Kp由小到大变化，观察系统的响应，直至响应速度快，且有一定范围的超调为止。如果系统静差在规定范围之内，且响应曲线已满足设计要求，那么只需用纯比例调节器即可。

（2） 积分部分整定。如果比例控制系统的静差达不到设计要求，这时可以加入积分作用。在整定时将积分系数KI由小逐渐增加（积分作用就逐渐增强），观察输出，系统的静差应逐渐减少直至消除（在性能指标要求下）。反复试验几次，直到消除静差的速度满意为止。注意这时的超调量会比原来加大，可能需要适当降低一些比例系数Kp。

（3） 微分部分整定。若使用比例积分（PI）控制器经反复调整仍达不到设计要求，应考虑加入微分作用。整定时先将微分系数KD从零逐渐增加（微分作用逐渐增强），观察超调量和稳定性，同时相应地微调比例系数Kp、积分系数KI，逐步试凑，直到满意为止。注意，在设计控制系统时，应使微分环节为实际微分环节，而不可以是理想微分环节。

 

这种方法的基本程序是先根据运行经验，确定一组调节器参数，并将系统投入闭环运行，然后人为地加入阶跃扰动（如改变调节器的给定值），观察被调量或调节器输出的阶跃响应曲线。若认为控制质量不满意，则根据各整定参数对控制过程的影响改变调节器参数。这样反复试验，直到满意为止。

      经验法简单可*，但需要有一定现场运行经验，整定时易带有主观片面性。当采用PID调节器时，有多个整定参数，反复试凑的次数增多，不易得到最佳整定参数。

 

      下面以PID调节器为例，具体说明经验法的整定步骤：

      ⑴让调节器参数积分系数S0=0，实际微分系数k=0，控制系统投入闭环运行，由小到大改变比例系数S1，让扰动信号作阶跃变化，观察控制过程，直到获得满意的控制过程为止。

      ⑵取比例系数S1为当前的值乘以0.83，由小到大增加积分系数S0，同样让扰动信号作阶跃变化，直至求得满意的控制过程。

      ⑶积分系数S0保持不变，改变比例系数S1，观察控制过程有无改善，如有改善则继续调整，直到满意为止。否则，将原比例系数S1增大一些，再调整积分系数S0，力求改善控制过程。如此反复试凑，直到找到满意的比例系数S1和积分系数S0为止。

      ⑷引入适当的实际微分系数k和实际微分时间TD，此时可适当增大比例系数S1和积分系数S0。和前述步骤相同，微分时间的整定也需反复调整，直到控制过程满意为止。

      注意：仿真系统所采用的PID调节器与传统的工业 PID调节器有所不同，各个参数之间相互隔离，互不影响，因而用其观察调节规律十分方便。

 

PID参数是根据控制对象的惯量来确定的。大惯量如：大烘房的温度控制，一般P可在10以上,I=3-10,D=1左右。小惯量如：一个小电机带一水泵进行压力闭环控制，一般只用PI控制。P=1-10,I=0.1-1,D=0,这些要在现场调试时进行修正的。

======================================================================
一般是这样子调节：先逐渐增大P增益，调到振荡发生前的最大值，
                   再逐渐减小I增益，调节到振荡发生前的最大值，
                   最后逐渐增大D增益，调节到振荡发生前的最大值。
======================================================================

 

 

下次写吧！还有很多细节没写，我会补上的

 

 

 

贴上我的控制代码：

 

#include "control.h"    
#include "filter.h"    
  /**************************************************************************
×÷ÕߣºÆ½ºâС³µÖ®¼Ò
ÎÒµÄÌÔ±¦Ð¡µê£ºhttp://shop114407458.taobao.com/
**************************************************************************/
int Balance_Pwm,Velocity_Pwm,Turn_Pwm;
u8 Flag_Target;
int Voltage_Temp,Voltage_Count,Voltage_All;
/**************************************************************************
º¯Êý¹¦ÄÜ£ºËùÓеĿØÖÆ´úÂ붼ÔÚÕâÀïÃæ
         5ms¶¨Ê±ÖжÏÓÉMPU6050µÄINTÒý½Å´¥·¢
         Ñϸñ±£Ö¤²ÉÑùºÍÊý¾Ý´¦ÀíµÄʱ¼äͬ²½                 
**************************************************************************/
int EXTI0_IRQHandler(void) 
{    
     if(PCin(0)==0)        
    {   
          EXTI_ClearITPendingBit(EXTI_Line0);                                                      //Çå³ýLINE5ÉϵÄÖжϱê־λ   
           Flag_Target=!Flag_Target;
          if(delay_flag==1)
             {
                 if(++delay_50==10)     delay_50=0,delay_flag=0;                     //¸øÖ÷º¯ÊýÌṩ50msµÄ¾«×¼ÑÓʱ
             }
          if(Flag_Target==1)                                                  //5ms¶ÁÈ¡Ò»´ÎÍÓÂÝÒǺͼÓËٶȼƵÄÖµ£¬¸ü¸ßµÄ²ÉÑùƵÂÊ¿ÉÒÔ¸ÄÉÆ¿¨¶ûÂüÂ˲¨ºÍ»¥²¹Â˲¨µÄЧ¹û
            {
                Get_Angle(Way_Angle);                                             //===¸üÐÂ×Ë̬            
      Voltage_Temp=Get_battery_volt();                                        //=====¶ÁÈ¡µç³Øµçѹ        
            Voltage_Count++;                                                    //=====ƽ¾ùÖµ¼ÆÊýÆ÷
            Voltage_All+=Voltage_Temp;                                          //=====¶à´Î²ÉÑùÀÛ»ý
            if(Voltage_Count==100) Voltage=Voltage_All/100,Voltage_All=0,Voltage_Count=0;//=====Çóƽ¾ùÖµ        
            return 0;                                                   
            }                                                                   //10ms¿ØÖÆÒ»´Î£¬ÎªÁ˱£Ö¤M·¨²âËÙµÄʱ¼ä»ù×¼£¬Ê×ÏȶÁÈ¡±àÂëÆ÷Êý¾Ý
            Encoder_Left=-Read_Encoder(2);                                      //===¶ÁÈ¡±àÂëÆ÷µÄÖµ£¬ÒòΪÁ½¸öµç»úµÄÐýתÁË180¶ÈµÄ£¬ËùÒÔ¶ÔÆäÖÐÒ»¸öÈ¡·´£¬±£Ö¤Êä³ö¼«ÐÔÒ»ÖÂ
            Encoder_Right=Read_Encoder(4);                                      //===¶ÁÈ¡±àÂëÆ÷µÄÖµ
          Get_Angle(Way_Angle);                                               //===¸üÐÂ×Ë̬    
            Read_Distane();                                                     //===»ñÈ¡³¬Éù²¨²âÁ¿¾àÀëÖµ
          if(Bi_zhang==0)Led_Flash(100);                                      //===LEDÉÁ˸;³£¹æģʽ 1s¸Ä±äÒ»´ÎָʾµÆµÄ״̬    
            if(Bi_zhang==1)Led_Flash(0);                                        //===LEDÉÁ˸;±ÜÕÏģʽ ָʾµÆ³£ÁÁ    
//            Key();                                                              //===ɨÃè°´¼ü״̬ µ¥»÷Ë«»÷¿ÉÒԸıäС³µÔËÐÐ״̬
             Balance_Pwm =balance(Angle_Balance,Gyro_Balance);                   //===ƽºâPID¿ØÖÆ    
          Velocity_Pwm=velocity(Encoder_Left,Encoder_Right);                  //===ËٶȻ·PID¿ØÖÆ     ¼Çס£¬Ëٶȷ´À¡ÊÇÕý·´À¡£¬¾ÍÊÇС³µ¿ìµÄʱºòÒªÂýÏÂÀ´¾ÍÐèÒªÔÙÅÜ¿ìÒ»µã
         Turn_Pwm    =turn(Encoder_Left,Encoder_Right,Gyro_Turn);            //===תÏò»·PID¿ØÖÆ     
           Moto1=Balance_Pwm-Velocity_Pwm+Turn_Pwm;                            //===¼ÆËã×óÂÖµç»ú×îÖÕPWM
           Moto2=Balance_Pwm-Velocity_Pwm-Turn_Pwm;                            //===¼ÆËãÓÒÂÖµç»ú×îÖÕPWM
           Xianfu_Pwm();                                                       //===PWMÏÞ·ù
//            if(Pick_Up(Acceleration_Z,Angle_Balance,Encoder_Left,Encoder_Right))//===¼ì²éÊÇ·ñС³µ±»ÄÇÆð
//            Flag_Stop=1;                                                          //===Èç¹û±»ÄÃÆð¾Í¹Ø±Õµç»ú
//            if(Put_Down(Angle_Balance,Encoder_Left,Encoder_Right))              //===¼ì²éÊÇ·ñС³µ±»·ÅÏÂ
//            Flag_Stop=0;                                                          //===Èç¹û±»·ÅϾÍÆô¶¯µç»ú
      if(Turn_Off(Angle_Balance,Voltage)==0)                              //===Èç¹û²»´æÔÚÒì³£
             Set_Pwm(Moto1,Moto2);                                               //===¸³Öµ¸øPWM¼Ä´æÆ÷  
    }           
     return 0;      
} 

/**************************************************************************
º¯Êý¹¦ÄÜ£ºÖ±Á¢PD¿ØÖÆ
Èë¿Ú²ÎÊý£º½Ç¶È¡¢½ÇËÙ¶È
·µ»Ø  Öµ£ºÖ±Á¢¿ØÖÆPWM
×÷    ÕߣºÆ½ºâС³µÖ®¼Ò
**************************************************************************/
int balance(float Angle,float Gyro)
{  
   float Bias;
     int balance;
     Bias=Angle-ZHONGZHI;       //===Çó³öƽºâµÄ½Ç¶ÈÖÐÖµ ºÍ»úеÏà¹Ø
     balance=Balance_Kp*Bias+Gyro*Balance_Kd;   //===¼ÆËãƽºâ¿ØÖƵĵç»úPWM  PD¿ØÖÆ   kpÊÇPϵÊý kdÊÇDϵÊý 
     return balance;
}

/**************************************************************************
º¯Êý¹¦ÄÜ£ºËÙ¶ÈPI¿ØÖÆ ÐÞ¸ÄÇ°½øºóÍËËٶȣ¬ÇëÐÞTarget_Velocity£¬±ÈÈ磬¸Ä³É60¾Í±È½ÏÂýÁË
Èë¿Ú²ÎÊý£º×óÂÖ±àÂëÆ÷¡¢ÓÒÂÖ±àÂëÆ÷
·µ»Ø  Öµ£ºËٶȿØÖÆPWM
×÷    ÕߣºÆ½ºâС³µÖ®¼Ò
**************************************************************************/
int velocity(int encoder_left,int encoder_right)
{  
     static float Velocity,Encoder_Least,Encoder,Movement;
      static float Encoder_Integral,Target_Velocity;
      //=============Ò£¿ØÇ°½øºóÍ˲¿·Ö=======================// 
      if(Bi_zhang==1&&Flag_sudu==1)  Target_Velocity=45;                 //Èç¹û½øÈë±ÜÕÏģʽ,×Ô¶¯½øÈëµÍËÙģʽ
    else                              Target_Velocity=110;                 
        if(1==Flag_Qian)        Movement=Target_Velocity/Flag_sudu;             //===Ç°½ø±ê־λÖÃ1 
        else if(1==Flag_Hou)    Movement=-Target_Velocity/Flag_sudu;         //===ºóÍ˱ê־λÖÃ1
      else  Movement=0;    
      if(Bi_zhang==1&&Distance<500&&Flag_Left!=1&&Flag_Right!=1)        //±ÜÕϱê־λÖÃ1ÇÒ·ÇÒ£¿ØתÍäµÄʱºò£¬½øÈë±ÜÕÏģʽ
      Movement=-Target_Velocity/Flag_sudu;
   //=============ËÙ¶ÈPI¿ØÖÆÆ÷=======================//    
        Encoder_Least =(Encoder_Left+Encoder_Right)-0;                    //===»ñÈ¡×îÐÂËÙ¶ÈÆ«²î==²âÁ¿Ëٶȣ¨×óÓÒ±àÂëÆ÷Ö®ºÍ£©-Ä¿±êËٶȣ¨´Ë´¦ÎªÁ㣩 
        Encoder *= 0.8;                                                        //===Ò»½×µÍͨÂ˲¨Æ÷       
        Encoder += Encoder_Least*0.2;                                        //===Ò»½×µÍͨÂ˲¨Æ÷    
        Encoder_Integral +=Encoder;                                       //===»ý·Ö³öλÒÆ »ý·Öʱ¼ä£º10ms
        Encoder_Integral=Encoder_Integral-Movement;                       //===½ÓÊÕÒ£¿ØÆ÷Êý¾Ý£¬¿ØÖÆÇ°½øºóÍË
        if(Encoder_Integral>10000)      Encoder_Integral=10000;             //===»ý·ÖÏÞ·ù
        if(Encoder_Integral<-10000)    Encoder_Integral=-10000;              //===»ý·ÖÏÞ·ù    
        Velocity=Encoder*Velocity_Kp+Encoder_Integral*Velocity_Ki;                          //===ËٶȿØÖÆ    
        if(Turn_Off(Angle_Balance,Voltage)==1||Flag_Stop==1)   Encoder_Integral=0;      //===µç»ú¹Ø±ÕºóÇå³ý»ý·Ö
      return Velocity;
}




/**************************************************************************
º¯Êý¹¦ÄÜ£º×ªÏò¿ØÖÆ  ÐÞ¸ÄתÏòËٶȣ¬ÇëÐÞ¸ÄTurn_Amplitude¼´¿É
Èë¿Ú²ÎÊý£º×óÂÖ±àÂëÆ÷¡¢ÓÒÂÖ±àÂëÆ÷¡¢ZÖáÍÓÂÝÒÇ
·µ»Ø  Öµ£º×ªÏò¿ØÖÆPWM
×÷    ÕߣºÆ½ºâС³µÖ®¼Ò
**************************************************************************/
int turn(int encoder_left,int encoder_right,float gyro)//תÏò¿ØÖÆ
{
     static float Turn_Target,Turn,Encoder_temp,Turn_Convert=0.9,Turn_Count;
      float Turn_Amplitude=88/Flag_sudu,Kp=42,Kd=0;     
      //=============Ò£¿Ø×óÓÒÐýת²¿·Ö=======================//
      if(1==Flag_Left||1==Flag_Right)                      //ÕâÒ»²¿·ÖÖ÷ÒªÊǸù¾ÝÐýתǰµÄËٶȵ÷ÕûËٶȵÄÆðʼËٶȣ¬Ôö¼ÓС³µµÄÊÊÓ¦ÐÔ
        {
            if(++Turn_Count==1)
            Encoder_temp=myabs(encoder_left+encoder_right);
            Turn_Convert=50/Encoder_temp;
            if(Turn_Convert<0.6)Turn_Convert=0.6;
            if(Turn_Convert>3)Turn_Convert=3;
        }    
      else
        {
            Turn_Convert=0.9;
            Turn_Count=0;
            Encoder_temp=0;
        }            
        if(1==Flag_Left)               Turn_Target-=Turn_Convert;
        else if(1==Flag_Right)         Turn_Target+=Turn_Convert; 
        else Turn_Target=0;
    if(Turn_Target>Turn_Amplitude)  Turn_Target=Turn_Amplitude;    //===תÏòËÙ¶ÈÏÞ·ù
      if(Turn_Target<-Turn_Amplitude) Turn_Target=-Turn_Amplitude;
        if(Flag_Qian==1||Flag_Hou==1)  Kd=0.5;        
        else Kd=0;   //תÏòµÄʱºòÈ¡ÏûÍÓÂÝÒǵľÀÕý ÓеãÄ£ºýPIDµÄ˼Ïë
      //=============תÏòPD¿ØÖÆÆ÷=======================//
        Turn=-Turn_Target*Kp -gyro*Kd;                 //===½áºÏZÖáÍÓÂÝÒǽøÐÐPD¿ØÖÆ
      return Turn;
}

/**************************************************************************
º¯Êý¹¦ÄÜ£º¸³Öµ¸øPWM¼Ä´æÆ÷
Èë¿Ú²ÎÊý£º×óÂÖPWM¡¢ÓÒÂÖPWM
·µ»Ø  Öµ£ºÎÞ
**************************************************************************/
void Set_Pwm(int moto1,int moto2)
{
        if(moto1<0)            AIN2=1,            AIN1=0;
            else               AIN2=0,            AIN1=1;
            PWMA=myabs(moto1);
          if(moto2<0)    BIN1=0,            BIN2=1;
            else        BIN1=1,            BIN2=0;
            PWMB=myabs(moto2);    
}

/**************************************************************************
º¯Êý¹¦ÄÜ£ºÏÞÖÆPWM¸³Öµ 
Èë¿Ú²ÎÊý£ºÎÞ
·µ»Ø  Öµ£ºÎÞ
**************************************************************************/
void Xianfu_Pwm(void)
{    
      int Amplitude=6900;    //===PWMÂú·ùÊÇ7200 ÏÞÖÆÔÚ6900
      if(Flag_Qian==1)  Moto1-=DIFFERENCE;  //DIFFERENCEÊÇÒ»¸öºâÁ¿Æ½ºâС³µµç»úºÍ»úе°²×°²îÒìµÄÒ»¸ö±äÁ¿¡£Ö±½Ó×÷ÓÃÓÚÊä³ö£¬ÈÃС³µ¾ßÓиüºÃµÄÒ»ÖÂÐÔ¡£
      if(Flag_Hou==1)   Moto2+=DIFFERENCE;
    if(Moto1<-Amplitude) Moto1=-Amplitude;    
        if(Moto1>Amplitude)  Moto1=Amplitude;    
      if(Moto2<-Amplitude) Moto2=-Amplitude;    
        if(Moto2>Amplitude)  Moto2=Amplitude;        
    
}
/**************************************************************************
º¯Êý¹¦ÄÜ£º°´¼üÐÞ¸ÄС³µÔËÐÐ״̬ 
Èë¿Ú²ÎÊý£ºÎÞ
·µ»Ø  Öµ£ºÎÞ
**************************************************************************/
/*void Key(void)
{    
    u8 tmp,tmp2;
    tmp=click_N_Double(50); 
    if(tmp==1)Flag_Stop=!Flag_Stop;//µ¥»÷¿ØÖÆС³µµÄÆôÍ£
    if(tmp==2)Flag_Show=!Flag_Show;//Ë«»÷¿ØÖÆС³µµÄÏÔʾ״̬
    tmp2=Long_Press();                   
  if(tmp2==1) Bi_zhang=!Bi_zhang;        //³¤°´¿ØÖÆС³µÊÇ·ñ½øÈ볬Éù²¨±ÜÕÏģʽ 
}*/

/**************************************************************************
º¯Êý¹¦ÄÜ£ºÒì³£¹Ø±Õµç»ú
Èë¿Ú²ÎÊý£ºÇã½ÇºÍµçѹ
·µ»Ø  Öµ£º1£ºÒì³£  0£ºÕý³£
**************************************************************************/
u8 Turn_Off(float angle, int voltage)
{
        u8 temp;
            if(angle<-40||angle>40||1==Flag_Stop||voltage<1110)//µç³ØµçѹµÍÓÚ11.1V¹Ø±Õµç»ú
            {                                                     //===Çã½Ç´óÓÚ40¶È¹Ø±Õµç»ú
      temp=0;                                            //===Flag_StopÖÃ1¹Ø±Õµç»ú
//            AIN1=0;                                            
//            AIN2=1;
//            BIN1=0;
//            BIN2=1;
      }
            else
      temp=0;
      return temp;            
}
    
/**************************************************************************
º¯Êý¹¦ÄÜ£º»ñÈ¡½Ç¶È ÈýÖÖËã·¨¾­¹ýÎÒÃǵĵ÷У£¬¶¼·Ç³£ÀíÏë 
Èë¿Ú²ÎÊý£º»ñÈ¡½Ç¶ÈµÄËã·¨ 1£ºDMP  2£º¿¨¶ûÂü 3£º»¥²¹Â˲¨
·µ»Ø  Öµ£ºÎÞ
**************************************************************************/
void Get_Angle(u8 way)
{ 
        float Accel_Y,Accel_X,Accel_Z,Gyro_Y,Gyro_Z;
           Temperature=Read_Temperature();      //===¶ÁÈ¡MPU6050ÄÚÖÃζȴ«¸ÐÆ÷Êý¾Ý£¬½üËƱíʾÖ÷°åζȡ£
        if(way==1)                           //===DMPµÄ¶ÁÈ¡ÔÚÊý¾Ý²É¼¯ÖжÏÌáÐѵÄʱºò£¬Ñϸñ×ñѭʱÐòÒªÇó
            {    
                    Read_DMP();                      //===¶ÁÈ¡¼ÓËٶȡ¢½ÇËٶȡ¢Çã½Ç
                    Angle_Balance=Pitch;             //===¸üÐÂƽºâÇã½Ç
                    Gyro_Balance=gyro[1];            //===¸üÐÂƽºâ½ÇËÙ¶È
                    Gyro_Turn=gyro[2];               //===¸üÐÂתÏò½ÇËÙ¶È
                  Acceleration_Z=accel[2];         //===¸üÐÂZÖá¼ÓËٶȼÆ
            }            
      else
      {
            Gyro_Y=(I2C_ReadOneByte(devAddr,MPU6050_RA_GYRO_YOUT_H)<<8)+I2C_ReadOneByte(devAddr,MPU6050_RA_GYRO_YOUT_L);    //¶ÁÈ¡YÖáÍÓÂÝÒÇ
            Gyro_Z=(I2C_ReadOneByte(devAddr,MPU6050_RA_GYRO_ZOUT_H)<<8)+I2C_ReadOneByte(devAddr,MPU6050_RA_GYRO_ZOUT_L);    //¶ÁÈ¡ZÖáÍÓÂÝÒÇ
          Accel_X=(I2C_ReadOneByte(devAddr,MPU6050_RA_ACCEL_XOUT_H)<<8)+I2C_ReadOneByte(devAddr,MPU6050_RA_ACCEL_XOUT_L); //¶ÁÈ¡XÖá¼ÓËٶȼÆ
          Accel_Z=(I2C_ReadOneByte(devAddr,MPU6050_RA_ACCEL_ZOUT_H)<<8)+I2C_ReadOneByte(devAddr,MPU6050_RA_ACCEL_ZOUT_L); //¶ÁÈ¡ZÖá¼ÓËٶȼÆ
          if(Gyro_Y>32768)  Gyro_Y-=65536;                       //Êý¾ÝÀàÐÍת»»  Ò²¿Éͨ¹ýshortÇ¿ÖÆÀàÐÍת»»
            if(Gyro_Z>32768)  Gyro_Z-=65536;                       //Êý¾ÝÀàÐÍת»»
          if(Accel_X>32768) Accel_X-=65536;                      //Êý¾ÝÀàÐÍת»»
          if(Accel_Z>32768) Accel_Z-=65536;                      //Êý¾ÝÀàÐÍת»»
            Gyro_Balance=-Gyro_Y;                                  //¸üÐÂƽºâ½ÇËÙ¶È
           Accel_Y=atan2(Accel_X,Accel_Z)*180/PI;                 //¼ÆËãÇã½Ç    
          Gyro_Y=Gyro_Y/16.4;                                    //ÍÓÂÝÒÇÁ¿³Ìת»»    
      if(Way_Angle==2)              Kalman_Filter(Accel_Y,-Gyro_Y);//¿¨¶ûÂüÂ˲¨    
            else if(Way_Angle==3)   Yijielvbo(Accel_Y,-Gyro_Y);    //»¥²¹Â˲¨
        Angle_Balance=angle;                                   //¸üÐÂƽºâÇã½Ç
            Gyro_Turn=Gyro_Z;                                      //¸üÐÂתÏò½ÇËÙ¶È
            Acceleration_Z=Accel_Z;                                //===¸üÐÂZÖá¼ÓËÙ¶È¼Æ    
        }
}
/**************************************************************************
º¯Êý¹¦ÄÜ£º¾ø¶ÔÖµº¯Êý
Èë¿Ú²ÎÊý£ºint
·µ»Ø  Öµ£ºunsigned int
**************************************************************************/
int myabs(int a)
{            
      int temp;
        if(a<0)  temp=-a;  
      else temp=a;
      return temp;
}
/**************************************************************************
º¯Êý¹¦ÄÜ£º¼ì²âС³µÊÇ·ñ±»ÄÃÆð
Èë¿Ú²ÎÊý£ºint
·µ»Ø  Öµ£ºunsigned int
**************************************************************************/
//int Pick_Up(float Acceleration,float Angle,int encoder_left,int encoder_right)
//{            
//     static u16 flag,count0,count1,count2;
//    if(flag==0)                                                                   //µÚÒ»²½
//     {
//          if(myabs(encoder_left)+myabs(encoder_right)<30)                         //Ìõ¼þ1£¬Ð¡³µ½Ó½ü¾²Ö¹
//                count0++;
//        else 
//        count0=0;        
//        if(count0>10)                
//            flag=1,count0=0; 
//     } 
//     if(flag==1)                                                                  //½øÈëµÚ¶þ²½
//     {
//            if(++count1>200)       count1=0,flag=0;                                 //³¬Ê±²»Ôٵȴý2000ms
//          if(Acceleration>26000&&(Angle>(-20+ZHONGZHI))&&(Angle<(20+ZHONGZHI)))   //Ìõ¼þ2£¬Ð¡³µÊÇÔÚ0¶È¸½½ü±»ÄÃÆð
//            flag=2; 
//     } 
//     if(flag==2)                                                                  //µÚÈý²½
//     {
//          if(++count2>100)       count2=0,flag=0;                                   //³¬Ê±²»Ôٵȴý1000ms
//        if(myabs(encoder_left+encoder_right)>135)                                 //Ìõ¼þ3£¬Ð¡³µµÄÂÖÌ¥ÒòΪÕý·´À¡´ïµ½×î´óµÄתËÙ   
//      {
//                flag=0;                                                                                     
//                return 1;                                                               //¼ì²âµ½Ð¡³µ±»ÄÃÆð
//            }
//     }
//    return 0;
//}
/**************************************************************************
º¯Êý¹¦ÄÜ£º¼ì²âС³µÊÇ·ñ±»·ÅÏÂ
Èë¿Ú²ÎÊý£ºint
·µ»Ø  Öµ£ºunsigned int
**************************************************************************/
//int Put_Down(float Angle,int encoder_left,int encoder_right)
//{            
//     static u16 flag,count;     
//     if(Flag_Stop==0)                           //·ÀÖ¹Îó¼ì      
//   return 0;                     
//     if(flag==0)                                               
//     {
//          if(Angle>(-10+ZHONGZHI)&&Angle<(10+ZHONGZHI)&&encoder_left==0&&encoder_right==0)         //Ìõ¼þ1£¬Ð¡³µÊÇÔÚ0¶È¸½½üµÄ
//            flag=1; 
//     } 
//     if(flag==1)                                               
//     {
//          if(++count>50)                                          //³¬Ê±²»Ôٵȴý 500ms
//          {
//                count=0;flag=0;
//          }
//        if(encoder_left>3&&encoder_right>3&&encoder_left<60&&encoder_right<60)                //Ìõ¼þ2£¬Ð¡³µµÄÂÖÌ¥ÔÚδÉϵçµÄʱºò±»ÈËΪת¶¯  
//      {
//                flag=0;
//                flag=0;
//                return 1;                                             //¼ì²âµ½Ð¡³µ±»·ÅÏÂ
//            }
//     }
//    return 0;
//}

 

转载于:https://www.cnblogs.com/baiji/p/11552475.html