急！！！！PID控制算法改进

nba232412 · 发表于 2009-5-7 23:17:16

本帖最后由 whut_wj 于 2009-5-9 08:49 编辑

把下面的PID算法改成积分分离PID或者PID增量式算法  我查资料理论上看懂了可是不知道怎么加到程序上  下面的程序是对了请大家帮帮忙  谢谢

#include <hidef.h>    /* common defines and macros */
#include <mc9s12dg128.h>    /* derivative information */
#include "PWM.h"
#include "Bang.h"
#include "video.h"
#include "Image_process.h"

#define  Kp 3
#define  Kd 0
#define  Speed_base 90  //////////////////////////////////////////////////////
// #define  Speed_min 50
#define  Angle_max 30
#define  T_ks  3
#define  Ksc 5
#define  Ksd 6
#define  Kss 7
// #define  Line_Max  50
// #define  Line_Min  10
unsigned char Line_Max,Speed_up,Speed_min,Speed_s;
signed char K_line,U=0; //中间变量
signed char Out_angle,Out_speed;          //输出量
signed char K_line0=0;
signed char K_line1=0;
signed int  Out_ang0=39,Out_ang;
unsigned char Line_middle=39,Y_X_max,Y_X_min,Y_max,Counter=T_ks,D=0,S=0,C=0;
unsigned char Ks=5,Flag=0,M,N;
unsigned char Now_line ;

//*****************************************************************************************
void Out_sp_ang(unsigned char Imagepoint[])
{
unsigned char i,j=0,X_max=0,X_min=255;
signed int  u;

if(Error==0)

  {
   Image_process(&Imagepoint[0]);

for(i=FirstLine;i<57;i++)
   {
   if(Imagepoint==255)
   break;
   }
Y_max=i-1;

if(Y_max>9)
{
if(Now_Speed<=Speed_min-10)
Now_line=Line_Max+10;
else
Now_line=Line_Max+(Now_Speed-Speed_min)/3;
if(Imagepoint[Now_line]<128)  //第Now_line行有黑点(若第i行无黑点，则Imagepoint=255)
   K_line=Imagepoint[Now_line]-Line_middle;
else
{
for(i=10;i<Now_line;i++)
      { if(Imagepoint<128&&Imagepoint[i+1]==255)    //第i行有黑点
         {
            K_line=(Imagepoint-Line_middle)*Now_line/i;
            break;
         }
         else j=1;
      }
}

/////////////////////////////////////速度////////////////////////////////////
if(Track==0)
{ Ks=Ksd;
if(Out_speed>Speed_base+Speed_up)
Out_speed-=10;
  if(Out_speed<Speed_base+Speed_up)
Out_speed+=10;

// if(Out_speed<Speed_base+Speed_up)
//    Out_speed+=20;
// if(Out_speed>Speed_base+Speed_up)
//    Out_speed=Speed_base+Speed_up;
//
// Out_angle=K_line*Kp/Ks;
  }
if(Track==1)
  {Ks=Kss;
  if(Out_speed>Speed_s)
Out_speed-=10;
  if(Out_speed<Speed_s)
Out_speed+=10;

//  Out_angle=K_line*Kp/Ks;
  }
if(Track==2)
  { Ks=Ksc;
   if(K_line>0)
   { u=Out_angle ;
      if(Out_speed>Speed_base+Speed_up-(unsigned char)u*u/80)
         Out_speed-=10;
      else Out_speed+=6;

//       if(Now_Speed>60)
//       Out_angle=K_line*Kp/Ks+(Now_Speed-60)/3;
//             else Out_angle=K_line*Kp/Ks ;


//       else
//          Out_speed=Speed_base-K_line;
   }


else
   { u=-Out_angle ;
      if(Out_speed>Speed_base+Speed_up-(unsigned char)u*u/80)
         Out_speed-=10;
      else Out_speed+=6;
//       if(Now_Speed>60)
//       Out_angle=K_line*Kp/Ks+(Now_Speed-Speed_min-1)/15;
//       else Out_angle=K_line*Kp/Ks ;
//       else
//          Out_speed=Speed_base+K_line;
   }

  }


/////////////////////////////////////////舵机///////////////////////////////////
   Out_angle=K_line*Kp/Ks;
// {
//    Out_angle=K_line*Kp/Ks+Kd*(K_line+K_line1-2*K_line0);
//    K_line1=K_line0;
//    K_line0=K_line ;
// }
}
if(Y_max>32&&Y_max<40)
{

  if(C<T_ks)
   C++;
  else C=T_ks;
  D=0;
}
else if(Y_max>40&&Y_max<54)
{

  if(D<T_ks)
   D++;
  else D=T_ks;
  C=0;
}
else{C=0;D=0;}

      if(C==T_ks)
      {

      if(Out_speed<Speed_base+Speed_up-10)
      Out_speed+=6;
      else Out_speed-=5;
      }

      if(D==T_ks)
      {

      if(Out_speed<Speed_base+Speed_up)
      Out_speed+=6;
      else Out_speed-=5;
      }


if(j==1)
{ if(Out_speed>Speed_min)
         Out_speed-=12;
      else
         Out_speed=Speed_min;
      if(Out_angle>0)
         Out_angle+=2;
      if(Out_angle<=0)
         Out_angle-=2;

}

///////////////////////////////////////出错////////////////////////////////////
}
else
{ if(Out_speed>Speed_min)
         Out_speed-=12;
      else
         Out_speed=Speed_min;

      if(Out_angle>0)
         Out_angle+=2;
      if(Out_angle<=0)
         Out_angle-=2;
}

//  PORTB=~Out_speed;

if(Now_Speed>Flag)
   Flag=Now_Speed ;
// PORTB=~Out_angle ;
//PORTB=~Track;
PORTB=~Y_max ;
//PORTB=~(Error*0xff);
}