中科因仑“3+1”工程特种兵精英论坛

标题: 基于PID的51巡线小车,PID调节完全没有效果，求指导！ [打印本页]

---

作者: 张衍波    时间: 2015-9-24 08:38
标题: 基于PID的51巡线小车,PID调节完全没有效果，求指导！
我刚接触PID不久，查阅了不少pid算法的资料，对pid基础有大致的了解，目前想把pid算法实现在巡线小车上。最开始我只用到了P来调节，调试过程中两个电机都震荡的很厉害，完全看不出调节的效果，从理论上分析我觉得有没有什么错误，出现的问题也是很奇怪，请大家帮我看看。
程序代码如下
/*软件实现的pwm调速h文件*/
#ifndef __pwm_h__                                        //单片机晶振12MHZ，pwm频率50hz（即周期20ms）
#define __pwm_h__                                        //定时器初值计算                        
#define uint int                                               //（1/100/100）/（12/12000000）=100
#define uchar char
uint timer;
sbit left1 =P2^0;        //电机控制端
sbit left2 =P2^1;
sbit right1=P2^2;
sbit right2=P2^3;
void init_timer0()
{
       TMOD|=0x01;
        TL0  = (65536-100);
       TH0  = (65536-100)>>8;         
        ET0  = 1;         
        EA   = 1;
        TR0  = 1;
}

void PWMSET(uint pwm1,uint pwm2,uint pwm3,uint pwm4)
{        
                if(timer < pwm1)        //改变pwm1控制电机1速度
                {
                        left1=1;
                }
                else
                {
                    left1=0;
                }

                if(timer < pwm2)        //改变pwm2控制电机2速度
                {
                        left2=1;
                }
                else
                {
                        left2=0;
                }               

                if(timer < pwm3)        //改变pwm3控制电机3速度
                {
                        right1=1;
                }
                else
                {
                        right1=0;
                }


                if(timer < pwm4)        //改变pwm4控制电机4速度
                {
                        right2=1;
                }
                else
                {
                        right2=0;
                }
        }
#endif
#include<reg52.h>
#include"math.h"
#include"pwm.h"
#include"stdio.h"
#include"intrins.h"
#define uint int
#define uchar char
uint sampledata=0;   //采样数据
uint offset;         //当前赛道位置
uint lastoffset[2]={0};//上一次赛道位置
static int SPEED1=100;  //电机pwm初值
static int SPEED2=100;
long clean1=0,clean2=1;  //采样周期控制位
sbit eye5=P1^0;       //传感器1-5
sbit eye4=P1^1;
sbit eye3=P1^2;
sbit eye2=P1^3;
sbit eye1=P1^4;
void delay(unsigned char d);
void uart_init();
void sample();
void confirlocation();
void filter();
void savedata();
float LocPIDCalc(uint NextPoint);
void motorctl();
struct PID
{
    uint    SetPoint;           //设定目标 Desired Value
    uint    SumError;             //误差累计
    float   Proportion;         //比例常数 Proportional Cons
//    float   Integral;           //积分常数 Integral Const
    float   Derivative;         //微分常数 Derivative Const
    uint    LastError;               //Error[-1]
    uint    PrevError;               //Error[-2]
}   locapid={0,0,1.5,0.01,0,0} ;

void delay(unsigned char e)   //误差 0us,延时ms
{
    unsigned char a,b,c,d;
        for(d=0;d<e;d++)
        {
     for(c=1;c>0;c--)
        for(b=142;b>0;b--)
            for(a=2;a>0;a--);
        }
}

void uart_init()
{
    SCON=0x40;                                        //串口通信工作方式1
        REN=1;                                                //允许接收
        TMOD|=0x20;                                        //定时器1的工作方式2
        TH1=0xfd;TL1=0xfd;               
        TI=1;                       //这里一定要注意
        TR1=1;
}
void sample()          //数据采样函数
{
       sampledata=0;
       if(eye1==1)
           sampledata|=0x01;
        if(eye2==1)
           sampledata|=0x02;
        if(eye3==1)
           sampledata|=0x04;
        if(eye4==1)
           sampledata|=0x08;
        if(eye5==1)
           sampledata|=0x10;
}
void confirlocation()        //确定赛道位置
{
          switch(sampledata)
        {
          case 0x10ffset=4; break; //10000最左边1个传感器检测到黑线
          case 0x18ffset=3; break; //11000最左边2个传感器检测到黑线
          case 0x08ffset=2; break; //01000
          case 0x0cffset=0; break; //01100
          case 0x04ffset=0; break; //00100
          case 0x06ffset=0;break; //00110
          case 0x02ffset=-2;break; //00010
          case 0x03ffset=-3;break; //00011
          case 0x01ffset=-4;break; //00001
          default:  offset=5; break; //其余情况包括跑道丢失于检测错误
        }
}
void filter() //赛道位置数据滤波（减少某次采样错误对系统的干扰
{
          if(offset==5)//滤除错误信号，没有检测到黑线（是需要保持上一次测量值的）
          {
             offset=lastoffset[0];
          }
          else if(abs(offset-lastoffset[0])>4)
          {
             offset=lastoffset[0];
          }
}
void savedata()
{
      static uint TCnt2=0;
          TCnt2++;
          lastoffset[0]=offset;
      if(TCnt2==20)   
           {        
             lastoffset[1]=lastoffset[0];                  
             TCnt2=0;     
           }                  
}
float LocPIDCalc(uint NextPoint)                //增量式pid
{
    float  Error,dError;
        struct PID *sptr;
        sptr=&locapid;
    Error = sptr->SetPoint - NextPoint;       //偏差
    sptr->SumError += Error;                  //积分
    dError = sptr->SumError -2* sptr->LastError+sptr->revError; //微分
    sptr->revError = sptr->LastError;
        sptr->LastError = Error;
        return (sptr->roportion * Error        //比例项
               /*+sptr->Integral * sptr->SumError *///积分项
               /*+sptr->Derivative * dError*/ );    //微分项
}
void motorctrl()    //电机pid算法控制
{
   if((offset>=2&&offset<=4)||(offset>=-4&&offset<=-2))
    {
          SPEED1=(int)(SPEED1+LocPIDCalc(offset));
          if((SPEED1>=170)||(SPEED1<=30))
            { SPEED1=100; }
          SPEED2=(int)(SPEED2-LocPIDCalc(offset));
          if((SPEED2>=170)||(SPEED2<=30))
            { SPEED2=100; }
        }  
        else
    {
           SPEED1=100;SPEED2=100;           
    }
        PWMSET(0,SPEED1,0,SPEED2);
        delay(5);
}
void main()
{
   init_timer0();
   uart_init();
   PWMSET(0,SPEED1,0,SPEED2);
   while(1)
   {  
          sample();                  //数据采样
          confirlocation();          //确认赛道信息
          filter();                          //滤波                        
          printf("SPEED1=  %d,SPEED2=  %d,offset=  %d,LocPIDCalc=  %.1f\n",SPEED1,SPEED2,offset,LocPIDCalc(offset));         
          while(clean2)
         {
          motorctrl();                    //电机控制
          clean2=0;
         }
          savedata();                  //保存赛道信息
   }
}
void timer0(void) interrupt 1                //100us
{
        TL0  = (65536-100);
       TH0  = (65536-100)>>8;
        timer++;
        clean1++;
        if(timer>200)  //PWM周期为 2/100S=20ms
                {
                        timer=0;
                }
        if(clean1==500)   //控制采样周期，50ms
  {
                clean2=1;
            clean1=0;
  }   
}
小车实物连接如图



因为网上都说先只用p来调节，直到小车大致可以巡线了再加上I和D,慢慢进行微调，所以我最开始只用的P转载

---

欢迎光临 中科因仑“3+1”工程特种兵精英论坛 (http://bbs.enlern.com/)

Powered by Discuz! X3.4