跪求！！急！！修改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[i]==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[i]=255)
      K_line=Imagepoint[Now_line]-Line_middle;
else
  {
   for(i=10;i<Now_line;i++)
       {    if(Imagepoint[i]<128&&Imagepoint[i+1]==255)     //第i行有黑点
            {
             K_line=(Imagepoint[i]-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);
}