arduino四路红外PID循迹小车

为了更好的采样我使用了arduino的中断，所以需要手动装一下中断库。主控是我自己画的，集成了驱动在板子上。但是其他驱动像L298N，TB6612都是可以代替的，更改下端口号即可。

代码如下：

#include <MsTimer2.h>
int sensor[5];
int moto1A = 5;   //我是四轮，左边两个电机并联，右边两个电机并联
int moto1B = 6;
int moto2A = 9;
int moto2B = 10;
int base = 100;   //基础速度
float err,last_err;
void moto_run(int x,int y);
void  line();
void  turn_left();
void  turn_right();
int  cacu_pwm();
void flash()           
{
    sensor[0]=digitalRead(A0); 
    sensor[1]=digitalRead(A1);
    sensor[2]=digitalRead(A2);
    sensor[3]=digitalRead(A3); 
    if(sensor[0] == 0&& sensor[1] == 0&& sensor[2] == 1&&sensor[3] == 0)  //0010
      err = 1;
    if(sensor[0] == 0&& sensor[1] == 0&& sensor[2] == 0&&sensor[3] == 1)  //0001
      err = 2;
    if(sensor[0] == 0&& sensor[1] == 1&& sensor[2] == 0&&sensor[3] == 0)  //0100
      err = -1;
    if(sensor[0] == 1&& sensor[1] == 0&& sensor[2] == 0&&sensor[3] == 0)  //1000
      err = -2;
}
int  cacu_pwm()            //pid运算部分
{
    int output;
    float kp=60,ki=0,kd=12;    //pid需要根据实际情况进行调节
    float P,I,D;
    P = err;
    I += err;
    D = err - last_err;
    output = kp*P + ki*I + kd*D; 
    last_err = err;
    return output;
}
void setup()
{
  Serial.begin(9600);
  pinMode(A0,INPUT);
  pinMode(A1,INPUT);
  pinMode(A2,INPUT);
  pinMode(A3,INPUT);
  pinMode(moto1A,OUTPUT);
  pinMode(moto1B,OUTPUT);
  pinMode(moto2A,OUTPUT);
  pinMode(moto2A,OUTPUT);
  MsTimer2::set(5, flash); //开启中断 5ms period
  MsTimer2::start();
}
void moto_run(int x,int y)           //电机控制部分
  {
     if(x>0)
     {
     analogWrite(moto1A,x); 
     analogWrite(moto1B,0); 
     }
     if(x<0)
     {
     analogWrite(moto1A,0); 
     analogWrite(moto1B,-x); 
     }
      if(y>0)
     {
     analogWrite(moto2A,y); 
     analogWrite(moto2B,0); 
     }
      if(y<0)
     {
     analogWrite(moto2A,0); 
     analogWrite(moto2B,-y); 
     }
  }
    void  line()   //直行
  {
    moto_run(100,100);  
  } 
    void  turn_left()   //左转
  {
    moto_run(-100,100);  
  } 
    void  turn_right()   //右转
  {
    moto_run(100,-100);  
  }
void loop()
{
     Serial.println(sensor[0]);
     Serial.println(sensor[1]);
     Serial.println(sensor[2]);
     Serial.println(sensor[3]);
     moto_run(base+cacu_pwm(),base-cacu_pwm());
}