求助此ＰＩＤ控制，怎么弄

PID()
{      
 LDN    inverting_PI                (初始值可以设定为1)
 ST      error_single_is_positive  (判断偏差为+还是-)
 LD      temp1
 SUB     pulse_error
 IsOverflow
 JMPCN  I_ACTION
 LD       inverting_PI
 ST       error_single_is_positive
 LD       pulse_error
 SUB     temp1
I_ACTION:
 LD       error_single_is_positive
 JMPC NEXT
 Integrate      =Pre_Integrate+errspeed;
 Derivative     =errspeed-Pre_Errspeed;    
NEXT:
 Integrate      =Pre_Integrate-errspeed;
     
 /***********计算PID的输出*********************

   Control_Output =speedKp*errspeed+speedTi*Integrate+speedTd*Derivative;     //PWM输出


 Pre_Errspeed   =errspeed;
 Pre_Integrate  =Integrate;
}

CAL()         // 定时做PID计算
{
   
     pulse_error=prepulse-pulse;   // pulse_error为脉冲差值
                   
     prepulse=pulse;            //PULSE为光电编码器测出的电机转速（脉冲个数），
   
   temp1=6*sin(temp);        // 模拟正弦函数曲线
   temp=temp+0.1;                  
   
   errspeed=((int)temp1-pulse_error;  // 期望值与当前值的差值， 用来作PID计算
           
      PID();      
  }

一点愚见,不知道能不能帮助你.

PID()
{      
 LDN    inverting_PI                (初始值可以设定为1)
 ST      error_single_is_positive  (判断偏差为+还是-)
 LD      temp1
 SUB     pulse_error
 ST       errspeed
 IsOverflow
 JMPCN  I_ACTION
 LD       inverting_PI
 ST       error_single_is_positive
 LD       pulse_error
 SUB     temp1
 ST       errspeed
I_ACTION:
 LD       error_single_is_positive
 JMPC NEXT
 Integrate      =Pre_Integrate+errspeed;
 Derivative     =errspeed-Pre_Errspeed;    
NEXT:
 Integrate      =Pre_Integrate-errspeed;
     
 /***********计算PID的输出*********************

   Control_Output =speedKp*errspeed+speedTi*Integrate+speedTd*Derivative;     //PWM输出


 Pre_Errspeed   =errspeed;
 Pre_Integrate  =Integrate;
}

CAL()         // 定时做PID计算
{
   
     pulse_error=prepulse-pulse;   // pulse_error为脉冲差值
                   
     prepulse=pulse;            //PULSE为光电编码器测出的电机转速（脉冲个数），
   
   temp1=6*sin(temp);        // 模拟正弦函数曲线
   temp=temp+0.1;                  
   
   errspeed=((int)temp1-pulse_error;  // 期望值与当前值的差值， 用来作PID计算
           
      PID();      
}