Algorithm template-PID

Int Error [3] = {0, 0}; int Pwm; int Kp, Ki, Kd; int speed [2]; int base_speed; void Mot_PID (int now_cnt, int target_cnt) {Error [2] = Error [1]; Error [1] = Error [0]; Error [0] = (target_cnt-now_cnt)> 3; pwm = Pwm + Kp * (Error [0]-Error [1]) + Ki * Error [0] + Kd * (Error [0]-2 * Error [1] + Error [2]); if (Pwm> 0) {if (Error [0]> 11) Pwm = 1000; if (Pwm> 1000) Pwm = 1000; PWMDTY23 = Pwm; PWMDTY45 = 0;} else {Pwm =-Pwm; // if (Error [0] <-11) Pwm = 1000; // bang-bang PWMDTY23 = 0; PWMDTY45 = Pwm ;}} // ************************************* void mot_Ctrl (int center_n) {int y; y = speed [0] * 10 + (speed [1]-speed [0]) * center_n/4; // get target speed Mot_PID (y );}