float Kp , Ki = 0, Kd ;                 				          //定义PID系数
float error = 0, P = 0, I = 0, D = 0, PID_value = 0;			//定义PID初始值
float previous_error = 0, previous_I = 0;          	      //定义PID初始值
static int initial_motor_speed = 240;			                //速度初始值（0-255）
int A[5] = {19, 18, 17, 16, 15};                          //定义初值
double L, R;                                              //定义左轮和右轮的速度
float a, b, c, d, p, m;                                   //定义初值
int ena = 5;                                              //定义使能A端的引脚
int enb = 10;                                             //定义使能B端的引脚

void setup()
{
  Serial.begin(9600);                                     //设置波特率

  //设置IN1、IN2、IN3、IN4的引脚为输出模式
  pinMode(6, OUTPUT);
  pinMode(7, OUTPUT);
  pinMode(8, OUTPUT);
  pinMode(9, OUTPUT);

//设置使能A端和B端为输出模式
  pinMode(ena, OUTPUT);
  pinMode(enb, OUTPUT);

//设置五路循迹传感器的5个接口为输入模式
  pinMode(A1, INPUT);
  pinMode(A2, INPUT);
  pinMode(A3, INPUT);
  pinMode(A4, INPUT);
  pinMode(A5, INPUT);
}

void loop()
{
  Kp =14.4;                                                 //定义Kp的值
  Kd = 38;             	                                    //定义Kd的值
  m = 0;                                                    //定义速度附加值
  a = 3, b = 6, c = 11, d = 18, p = 1;                      //定义初值
  trac_run();                                               //运动函数
}

//读取五路循迹传感器的状态的函数
void trac()
{
  unsigned char temp = 0b00000; 			                      //临时变量用于新一轮采集

  for (int i = 0; i < 5; i++)
  {
    temp |= digitalRead(A[i]) << i; 			                  //轮询5个传感器输出，并将查询结果转换为编码形式
  } 

  switch (temp)                                             //根据五路循迹传感器的状态赋予error的值
  {
    case 0b01111: error = -d; break;
    case 0b00111: error = -c; break;
    case 0b10111: error = -b; break;
    case 0b10011: error = -a, m = 10; break;
    case 0b11011: error = 0, m = 20; break;
    case 0b11001: error = a, m = 10; break;
    case 0b11101: error = b; break;
    case 0b11100: error = c; break;
    case 0b11110: error = d; break;
    default: p = 0; break;
  }
}

//进行PID算法运算函数
void pid()
{
  P = error;
  I = I + error;
  D = error - previous_error;
  PID_value = (Kp * P) + (Ki * I) + (Kd * D);
  previous_error = error;
}

//运动函数
void trac_run()
{
  trac();                                                 //对五路循迹传感器进行读取
  pid();                                                  //根据上述读取的结果进行PID运算

  L = p * ( initial_motor_speed + PID_value + m);         //计算左轮速度
  R = p * ( initial_motor_speed - PID_value + m);         //计算右轮速度

  //用于处理速度在0-255范围外的状态
  if (abs(L) > 255) L = L / abs(L) * 255;                 
  if (abs(R) > 255) R = R / abs(R) * 255;
  if (abs(L) == 0) L = 1;
  if (abs(R) == 0) R = 1;

  motor();                                                 //将速度赋予左右轮并进行运动
}

//根据L、R的状态赋予速度
void motor()
{
  if (L > 0 && R > 0)
  {
    analogWrite(ena, L);
    analogWrite(enb, R);
    motordir(0, 1, 1, 0);
  }
  else if (- L > 0 && - R > 0)
  {
    analogWrite(ena, -L);
    analogWrite(enb, -R);
    motordir(1, 0, 0, 1);
  }
  else if (- L > 0 && R > 0)
  {
    analogWrite(ena, -L);
    analogWrite(enb, R);
    motordir(0, 1, 0, 1);
  }
  else if (L > 0 && -R > 0)
  {
    analogWrite(ena, L);
    analogWrite(enb, -R);
    motordir(1, 0, 1, 0);
  }
  else
  {
    motordir(0, 0, 0, 0);
  }

  Serial.print("左轮");
  //Serial.println(L);
  Serial.print("右轮");
  //Serial.println(R);
}

//对IN1、IN2、IN3、IN4端口进行控制
void motordir(int out1, int out2, int out3, int out4)	
{
  digitalWrite(6, out1);
  digitalWrite(7, out2);
  digitalWrite(8, out3);
  digitalWrite(9, out4);
}