飞思卡尔（二）直立控制

转载请注明出处：http://blog.csdn.net/lxk7280

直立视频

2014/03/03 - -  2014/03/04  

Ps：某君天南海北胡玩了一通之后，某天回到实验室，突然发现某组的四轮光电车可以慢速在跑道上跑了，于是某人火了，决定奋发图强，怒刷直立。

通过查阅资料发现车模的直立和角度和角加速度有联系，于是花了几天通过Arduino把陀螺仪和加速度计结合调出角度和角加速度之后（因为Arduino的使用方便，调试传感器乃上上之选~强烈推荐。），决定结合PID算法中的P（比例）D（微分）控制让车子直立起来。

       第一步：通过AD采集观察一下硬件积分后的陀螺仪和加速度计，并通过多次采集之后确定准确的比例缩放，从而得到车模的准确的角度。下面附上一下处理程序（尚未滤波，AD初始化的一些基本的程序就不贴了，玩过单片机的人想必大家都会。）

for(i=0;i<2;i++){ADC_Value_Angle = ADC_GetConversionValue(ADC1_SE9_PB1); Sum_Value_Angle += ADC_Value_Angle;//ADC_Value_Acceleration = ADC_GetConversionValue(ADC1_SE8B_PC10); //program shut down.卡的原因是B通道只能通过硬件出发 PC10为B通道ADC_Value_Acceleration = ADC_GetConversionValue(ADC1_SE8_PB0);Sum_Value_Acceleration += ADC_Value_Acceleration;}/*修正角度值*/Right_Value_Angle = Sum_Value_Angle / 2.0;Right_Value_Angle -= 610;Right_Value_Angle *= ( 90.0 / 190.0 );Right_Value_Angle += 36;  //34Right_Value_Angle = - Right_Value_Angle;/*在第一行显示角度*/sprintf((char *)strBuffer,"%f",Right_Value_Angle);OLED_8x16(0,2,strBuffer);

      第二步：同上，Get 角速度。

for(i=0;i<2;i++){ADC_Value_Angle = ADC_GetConversionValue(ADC1_SE9_PB1); Sum_Value_Angle += ADC_Value_Angle;//ADC_Value_Acceleration = ADC_GetConversionValue(ADC1_SE8B_PC10); //program shut down.卡的原因是B通道只能通过硬件出发 PC10为B通道ADC_Value_Acceleration = ADC_GetConversionValue(ADC1_SE8_PB0);Sum_Value_Acceleration += ADC_Value_Acceleration;}/*修正角加速度值*/Right_Value_Acceleration = Sum_Value_Acceleration / 2.0;Right_Value_Acceleration = Right_Value_Acceleration / 5.1;Right_Value_Acceleration = Right_Value_Acceleration - 106 + 0.1176456 - 0.0960763 +0.292159;/*在第二行显示角加速度*/sprintf((char *)strBuffer,"%f",Right_Value_Acceleration);OLED_8x16(0,4,strBuffer);

     第三步：学习我亲爱的PID算法，先贴上一个贴吧上的某牛的关于PID的形象生动的介绍，个人觉得受益匪浅，屌爆了，远程谢谢此牛（尽管我不认识你，but感激你）。

制模型：你控制一个人让他以PID控制的方式走110步后停下。 
（1）P比例控制，就是让他走110步，他按照一定的步伐走到一百零几步（如108步）或100多步（如112步）就停了。 
说明： 
P比例控制是一种最简单的控制方式。其控制器的输出与输入误差信号成比例关系。当仅有比例控制时系统输出存在稳态误差（Steady-state error）。 


（2）PI积分控制，就是他按照一定的步伐走到112步然后回头接着走，走到108步位置时，然后又回头向110步位置走。在110步位置处来回晃几次，最后停在110步的位置。 
说明： 
在积分I控制中，控制器的输出与输入误差信号的积分成正比关系。对一个自动控制系统，如果在进入稳态后存在稳态误差，则称这个控制系统是有稳态误差的或简称有差系统（System with Steady-state Error）。为了消除稳态误差，在控制器中必须引入“积分项”。积分项对误差取决于时间的积分，随着时间的增加，积分项会增大。这样，即便误差很小，积分项也会随着时间的增加而加大，它推动控制器的输出增大使稳态误差进一步减小，直到等于零。因此，比例+积分（PI）控制器，可以使系统在进入稳态后无稳态误差。 


（3）PD微分控制，就是他按照一定的步伐走到一百零几步后，再慢慢地向110步的位置靠近，如果最后能精确停在110步的位置，就是无静差控制；如果停在110步附近（如109步或111步位置），就是有静差控制。 
说明： 
在微分控制D中，控制器的输出与输入误差信号的微分（即误差的变化率）成正比关系。
自动控制系统在克服误差的调节过程中可能会出现振荡甚至失稳，其原因是由于存在有较大惯性组件（环节）或有滞后（delay）组件，具有抑制误差的作用，其变化总是落后于误差的变化。解决的办法是使抑制误差作用的变化“超前”，即在误差接近零时，抑制误差的作用就应该是零。这就是说，在控制器中仅引入“比例P”项往往是不够的，比例项的作用仅是放大误差的幅值，而目前需要增加的是“微分项”，它能预测误差变化的趋势。这样，具有比例+微分的控制器，就能够提前使抑制误差的控制作用等于零，甚至为负值，从而避免了被控量的严重超调。所以对有较大惯性或滞后的被控对象，比例P+微分D（PD）控制器能改善系统在调节过程中的动态特性。


解释二：


　　小明接到这样一个任务：有一个水缸有点漏水(而且漏水的速度还不一定固定不变)，要求水面高度维持在某个位置，一旦发现水面高度低于要求位置，就要往水缸里加水。 小明接到任务后就一直守在水缸旁边，时间长就觉得无聊，就跑到房里看小说了，每30分钟来检查一次水面高度。水漏得太快，每次小明来检查时，水都快漏完了，离要求的高度相差很远，小明改为每3分钟来检查一次，结果每次来水都没怎么漏，不需要加水，来得太频繁做的是无用功。几次试验后，确定每10分钟来检查一次。这个检查时间就称为采样周期。 
　　开始小明用瓢加水，水龙头离水缸有十几米的距离，经常要跑好几趟才加够水，于是小明又改为用桶加，一加就是一桶，跑的次数少了，加水的速度也快了，但好几次将缸给加溢出了，不小心弄湿了几次鞋，小明又动脑筋，我不用瓢也不用桶，老子用盆，几次下来，发现刚刚好，不用跑太多次，也不会让水溢出。这个加水工具的大小就称为比例系数。
　　小明又发现水虽然不会加过量溢出了，有时会高过要求位置比较多，还是有打湿鞋的危险。他又想了个办法，在水缸上装一个漏斗，每次加水不直接倒进水缸，而是倒进漏斗让它慢慢加。这样溢出的问题解决了，但加水的速度又慢了，有时还赶不上漏水的速度。于是他试着变换不同大小口径的漏斗来控制加水的速度，最后终于找到了满意的漏斗。漏斗的时间就称为积分时间。 
　　小明终于喘了一口，但任务的要求突然严了，水位控制的及时性要求大大提高，一旦水位过低，必须立即将水加到要求位置，而且不能高出太多，否则不给工钱。小明又为难了！于是他又开努脑筋，终于让它想到一个办法，常放一盆备用水在旁边，一发现水位低了，不经过漏斗就是一盆水下去，这样及时性是保证了，但水位有时会高多了。他又在要求水面位置上面一点将水缸要求的水平面处凿一孔，再接一根管子到下面的备用桶里这样多出的水会从上面的孔里漏出来。这个水漏出的快慢就称为微分时间。

   看完PID的介绍之后，就开始通过PID的实例来学习和明白PID算法的流程，本屌搜啊搜，搜到一堆PID算法程序，but but 在万花之中，一个51写温控PID程序入我神目，我决心仔细分析，用心观察，然后发现真TMD的nice。下面献给大家这个示例，真的不错。

//温控PID程序#include<reg51.h>#include<intrins.h>#include<math.h>#include<string.h>struct PID {unsigned int SetPoint; // 设定目标 Desired Valueunsigned int Proportion; // 比例常数 Proportional Constunsigned int Integral; // 积分常数 Integral Constunsigned int Derivative; // 微分常数 Derivative Constunsigned int LastError; // Error[-1]unsigned int PrevError; // Error[-2]unsigned int SumError; // Sums of Errors};struct PID spid; // PID Control Structureunsigned int rout; // PID Response (Output)unsigned int rin; // PID Feedback (Input)sbit data1=P1^0;sbit clk=P1^1;sbit plus=P2^0;sbit subs=P2^1;sbit stop=P2^2;sbit output=P3^4;sbit DQ=P3^3;unsigned char flag,flag_1=0;unsigned char high_time,low_time,count=0;//占空比调节参数unsigned char set_temper=35;   unsigned char temper;        unsigned char i;unsigned char j=0;unsigned int s;/***********************************************************延时子程序,延时时间以12M晶振为准,延时时间为30us×time***********************************************************/void delay(unsigned char time){   unsigned char m,n;     for(n=0;n<time;n++)       for(m=0;m<2;m++){}}/***********************************************************写一位数据子程序***********************************************************/void write_bit(unsigned char bitval){    EA=0;      DQ=0;   /*拉低DQ以开始一个写时序*/    if(bitval==1)       {        _nop_();        DQ=1;    /*如要写1，则将总线置高*/        }      delay(5);    /*延时90us供DA18B20采样*/    DQ=1;   /*释放DQ总线*/    _nop_();    _nop_();   EA=1;   }/***********************************************************写一字节数据子程序***********************************************************/void write_byte(unsigned char val){unsigned char i;unsigned char temp;EA=0;TR0=0;for(i=0;i<8;i++)   /*写一字节数据，一次写一位*/      {       temp=val>>i;   /*移位操作，将本次要写的位移到最低位*/       temp=temp&1;       write_bit(temp);   /*向总线写该位*/       }   delay(7);    /*延时120us后*/// TR0=1;EA=1;}/***********************************************************读一位数据子程序***********************************************************/unsigned char read_bit(){unsigned char i,value_bit;EA=0;DQ=0;    /*拉低DQ，开始读时序*/_nop_();_nop_();DQ=1;    /*释放总线*/for(i=0;i<2;i++){} value_bit=DQ;EA=1;return(value_bit);}/***********************************************************读一字节数据子程序***********************************************************/unsigned char read_byte(){   unsigned char i,value=0;   EA=0;   for(i=0;i<8;i++)      {       if(read_bit())   /*读一字节数据，一个时序中读一次，并作移位处理*/         value|=0x01<<i;       delay(4);   /*延时80us以完成此次都时序，之后再读下一数据*/       }   EA=1;   return(value);}/***********************************************************复位子程序***********************************************************/unsigned char reset(){   unsigned char presence;   EA=0;   DQ=0;    /*拉低DQ总线开始复位*/   delay(30);    /*保持低电平480us*/   DQ=1;    /*释放总线*/   delay(3);      presence=DQ;    /*获取应答信号*/   delay(28);    /*延时以完成整个时序*/   EA=1;   return(presence);   /*返回应答信号，有芯片应答返回0,无芯片则返回1*/}/***********************************************************获取温度子程序***********************************************************/void get_temper(){unsigned char i,j;do{    i=reset();   /*复位*/}while(i!=0);   /*1为无反馈信号*/i=0xcc;   /*发送设备定位命令*/write_byte(i);i=0x44;   /*发送开始转换命令*/write_byte(i);delay(180);   /*延时*/do{i=reset();   /*复位*/}while(i!=0);  i=0xcc;   /*设备定位*/write_byte(i);i=0xbe;   /*读出缓冲区内容*/write_byte(i);j=read_byte(); i=read_byte();   i=(i<<4)&0x7f; s=(unsigned int)(j&0x0f);s=(s*100)/16;j=j>>4;temper=i|j;                                      /*获取的温度放在temper中*/}/*====================================================================================================Initialize PID Structure=====================================================================================================*/void PIDInit (struct PID *pp){memset ( pp,0,sizeof(struct PID));}/*====================================================================================================PID计算部分=====================================================================================================*/unsigned int PIDCalc( struct PID *pp, unsigned int NextPoint ){unsigned int dError,Error;Error = pp->SetPoint - NextPoint; // 偏差pp->SumError += Error; // 积分dError = pp->LastError - pp->PrevError; // 当前微分pp->PrevError = pp->LastError;pp->LastError = Error;return (pp->Proportion * Error // 比例项+ pp->Integral * pp->SumEror // 积分项+ pp->Derivative * dError); // 微分项}/***********************************************************温度比较处理子程序***********************************************************/compare_temper(){unsigned char i;     if(set_temper>temper)         {          if(set_temper-temper>1)                {              high_time=100;              low_time=0;             }          else             {               for(i=0;i<10;i++)                 { get_temper();                    rin = s; // Read Input                    rout = PIDCalc ( &spid,rin ); // Perform PID Interation                 }               if (high_time<=100)                 high_time=(unsigned char)(rout/800);               else                 high_time=100;               low_time= (100-high_time);             }         }      else if(set_temper<=temper)             {               if(temper-set_temper>0)                 {                   high_time=0;                   low_time=100;                 }               else               {                 for(i=0;i<10;i++)                 { get_temper();                    rin = s; // Read Input                    rout = PIDCalc ( &spid,rin ); // Perform PID Interation                 }                 if (high_time<100)                   high_time=(unsigned char)(rout/10000);                 else                   high_time=0;                 low_time= (100-high_time);               }             }     // else     //      {}    }/*****************************************************T0中断服务子程序，用于控制电平的翻转 ,40us*100=4ms周期******************************************************/void serve_T0() interrupt 1 using 1{   if(++count<=(high_time))     output=1;   else if(count<=100)     {      output=0;      }   else      count=0;   TH0=0x2f;   TL0=0xe0;   }/*****************************************************串行口中断服务程序，用于上位机通讯******************************************************/void serve_sio() interrupt 4 using 2{/*   EA=0;   RI=0;     i=SBUF;   if(i==2)      {       while(RI==0){}         RI=0;       set_temper=SBUF;         SBUF=0x02;         while(TI==0){}       TI=0;       }    else if(i==3)         {        TI=0;       SBUF=temper;        while(TI==0){}        TI=0;        }     EA=1;   */   }void disp_搜索1(unsigned char disp_num1[6]){     unsigned char n,a,m;     for(n=0;n<6;n++)      {     // k=disp_num1[n];       for(a=0;a<8;a++)         {          clk=0;           m=(disp_num1[n]&1);            disp_num1[n]=disp_num1[n]>>1;          if(m==1)             data1=1;          else             data1=0;          _nop_();          clk=1;          _nop_();          }       }

 Do You Think So？

   第四步：传感器采集的角度和角速度数据和PD算法的完美结合，必须先声明我的PD参数还不是调得很好，但个人觉得已经非常不错了，再过几天闭环起来相信就非常非常稳定了。可惜CSDN博客好像传不了视频（还是我不会）。所以只能附照给亲们看看了。

下面附上调成成功后拍的视频的链接：   

  请点击链接： 

       click here ：直立视频~~~~~~~~！！~~~~~~~~

     PS：我的参数值：P =  10      D  = 1.30;

         心情录：无商不奸。无商不奸。被一个老板摆了。今天其实是我火大的一天。吸气吸气~~