Arduino自走車PID轉速控制

#include <AFMotor.h>
AF_DCMotor motorLeft(2);
AF_DCMotor motorRight(3);

int DigitalPin = A5;     
int cnt = 0;  
int last = 0;
unsigned long timer;
unsigned long dt;
float rpm;
float bias, power, intergral_bias, last_bias;

const float Kp = 3.3;
const float Ki = 0.002;
const float Kd = 2.2;

int PID_motor(int rpm,int target){
  bias = -1*(rpm-target);//轉速越大，pwm越小
  intergral_bias = intergral_bias*0.7 + bias;//過去資料*0.7，避免累計過多
  power = Kp*bias + Ki*intergral_bias + -1*Kd*(bias-last_bias)+120;
  if(power > 255){
    power = 255;
  }
  if(power < 10){
    power = 10;
  }
  last_bias = bias;
  Serial.println(power);
  return power;
}

void setup()
{
  Serial.begin(115200);
  pinMode(DigitalPin, INPUT);  
  motorLeft.setSpeed(150);
  motorLeft.run(FORWARD);          
}
void loop()
{
  timer = millis();
  //Serial.println(timer); 
  while(cnt < 10){
  boolean D_val = digitalRead(DigitalPin);
  if(abs(last - D_val) == 1){
    cnt++;
  }
  last = D_val;
  }
  dt = millis() - timer;
  rpm = (60000/float(dt));
  //Serial.println("rpm= ");
  Serial.print(rpm); 
  Serial.print(" "); 
  cnt = 0;
  PID_motor(rpm,80);
  motorLeft.setSpeed(power);
}