用PID控制小电机

#include<TimerOne.h>
int NC = 4;
int ENA = 6;
int ENC_A = 2;
int ENC_B = 3;
int IN1 = 10;
int IN2 = 11;
int output_pwm;
int count = 0;
double rpm = 0,input_rspeed,output_rspeed=0;
int reduction_ratio = 20;//减速比
double err,preerr,prepreerr;
double goal_rspeed = 200;
double pid,Kp=0.6,Ki=0.1,Kd=2;
void setup() {
  // put your setup code here, to run once:
  Serial.begin(9600);
  pinMode(NC,OUTPUT);
  pinMode(IN1,OUTPUT);
  pinMode(IN2,OUTPUT);
  pinMode(ENA,OUTPUT);
  digitalWrite(NC,HIGH);
  digitalWrite(IN1,HIGH);
  digitalWrite(IN2,LOW);
  Timer1.initialize(50000);
  Timer1.attachInterrupt(timerIsr);
  pinMode(ENC_A,INPUT);
  pinMode(ENC_B,INPUT);
  attachInterrupt(0,Code1,CHANGE);
  attachInterrupt(1,Code2,CHANGE);
 }
void timerIsr()
{
  rpm = (double)count*20*60/260/4;
  count = 0;  
  input_rspeed = rpm;
  output_rspeed += PID(input_rspeed,goal_rspeed);
  output_pwm = 0.3 * output_rspeed;
}
double PID(double in, double goal)
{
  prepreerr = preerr;
  preerr = err;
  err = goal - in;
  pid += Kp*(err-preerr)+Ki*err+Kd*((err-preerr)-
  (preerr-prepreerr));
  return pid;
}
void Code1()
{
  if(digitalRead(ENC_A) == LOW)
  {
    if(digitalRead(ENC_B) == LOW)
    {
      count -= 1; 
    }
    if(digitalRead(ENC_B) == HIGH)
    {
      count += 1;
    }
  }
  if(digitalRead(ENC_A) == HIGH)
  {
    if(digitalRead(ENC_B) == HIGH)
    {
      count -= 1;
    }
    if(digitalRead(ENC_B) == LOW)
    {
      count += 1;  
    }
  }
}
void Code2()
{
  if(digitalRead(ENC_B) == LOW)
  {
    if(digitalRead(ENC_A) == HIGH)
    {
      count -= 1; 
    }
    if(digitalRead(ENC_A) == LOW)
    {
      count += 1;
    }
  }
  if(digitalRead(ENC_B) == HIGH)
  {
    if(digitalRead(ENC_A) == LOW)
    {
      count -= 1;
    }
    if(digitalRead(ENC_A) == HIGH)
    {
      count += 1;  
    }
  }
}
void loop() {
  // put your main code here, to run repeatedly:
  Serial.print("goal:");
  Serial.print(goal_rspeed);
  Serial.print("  rpm:");
  Serial.println(rpm);
  analogWrite(ENA,output_pwm);
}