s12电磁组舵机pid怎么写啊

△ABC≌△DEF

电磁组的pid算法怎么写啊，苦恼了很久了，想不出写的办法来呢，以下是自己的的程序，请指教
/////////////////////////////////////
////     电机   PID  定义
/////////////////////////////////////

int SetPoint=0; //设定目标 Desired Value
int  FeedBack=0;
float KKp=1.5; //比例常数 Proportional Const
float KKi=0;
float KKd=0.6; //微分常数 Derivative Const
signed int EE0=0;  //当前误差
signed int EE1=0; //Error[-1]
signed int EE2=0; //Error[-2]

signed int EError0=0,EError1=0;
signed int iiIncpid=0;
int sp=0;

///////////////////////////////
///    舵机 PID  
//////////////////////////////
//////////////////////////////////////  
int duoji_pid(int ch1,int ch2 )

{
   E0=ch1-ch2; //增量计算
   Error0=E0-E1;
   Error1=E1-E2;
   iIncpid=(int)(Ki*Error0+Kp*E0+Kd*(Error0-Error1));
   E1=E0;  
   E2=E1;   //存储误差，用于下次计算  
   return iIncpid;                                       
}

//-----PWM 初始化程序------//
void PWM_Init(void)
{
  PWME = 0x00;             //禁止
  PWMCTL = 0x70;           // no concatenation              控制寄存器设置01、23、45级联。                 
  PWMPRCLK = 0x00;         // 0000 0000 A = B = 24M         时钟预分频寄存器设置
  PWMSCLA = 12;            // SA = A/2/12 = 1000k           时钟设置
  PWMSCLB = 12;            // SB = B/2/12 = 1000K
  PWMCLK_PCLK1 = 1;        // PWM01-----SA               时钟源的选择
  PWMCLK_PCLK5 = 1;        // PWM45-----SA               时钟源的选择
  PWMCLK_PCLK3 = 1;        // PWM23-----SB               时钟源的选择
  PWMPOL_PPOL1 = 1;        // Duty=High Time                极性设置
  PWMPOL_PPOL5 = 1;        // Duty=High Time                极性设置
  PWMPOL_PPOL3 = 1;        // Duty=High Time                极性设置
  PWMCAE_CAE1 = 0;         // Left-aligned                  对齐方式设置
  PWMCAE_CAE5 = 0;         // Left-aligned                  对齐方式设置
  PWMCAE_CAE3 = 0;         // Left-aligned                  对齐方式设置
  PWMPER01 = 20000;        //周期20ms     50HZ
  PWMPER45 = 1000;         //Frequency=SA/1000=1K           周期寄存器设置
  PWMPER23 = 1000;         
  PWMDTY01 = 1580;            //占空比寄存器设置
  PWMDTY45 = 600;
  PWMDTY23 = 600;
  PWME = 0x3f;             //使能
}

//-----速度、角度控制-----//
uint jiaodu_ctrl(void)
{
  uint jiaodu;

  jiaodu=1580-duoji_pid(ATD0DR1L+ATD0DR2L,ATD0DR1L+ATD0DR2L) ;
  if (jiaodu>1780) jiaodu=1780;
  if (jiaodu<1380) jiaodu=1380;
  return jiaodu;  
}

不知道这样可否大神求指教

△ABC≌△DEF

？？？？求助啊