Freescale 单片机的PID控制直流电机

#define C 0.16
float KP = 8;
float KD = 0.01;
float KI = 1;
//float C=0.17;  // 车轮周长
float position;
float cPWMK;
float error1 = 0;
float error2 = 0;
float error3 = 0;
float speed(float P) {
float temp;
temp = 1.7-C/P;
return temp;
}
extern void driver_dcmotor(unsigned int pwm_input_dc){
      PWMDTY01 = 0;
      PWMDTY45=pwm_input_dc;
      
}
long PID(unsigned int target_pwm)
{
int PWM;
error1=speed(period);
cPWMK=KP*(error1-error2)+KI*error1+KD*(error1-2*error2+error3);
PWM =(int)(target_pwm+cPWMK);
error2 = error1;
error3 = error3 + error1;
return PWM;
}