ARDUINO2560版本的6路寻迹程序

此程序为比赛开发，采用外部中断作为防丢失的保险，PID调节，并有红外壁纸，指定距离停车功能。以下为源代码

#include <LiquidCrystal.h>
#include <Wire.h>
#include "Adafruit_PWMServoDriver.h"

Adafruit_PWMServoDriver pwm1 = Adafruit_PWMServoDriver(0X40);
Adafruit_PWMServoDriver pwm2 = Adafruit_PWMServoDriver(0X41);

//电机电压：6.1V左右的参数，如果不加积分项，可以循线跑，但是无法回到中间传感器位置。
float Kp , Ki , Kd ;
static float error = 0, P = 0, I = 0, D = 0, PID_value = 0;
float previous_error = 0, previous_I = 0;
static int initial_motor_speed ; //期望速度
int left_motor_speed = 0;
int right_motor_speed = 0;
uint8_t irs = 0;
static int a , b, c, d;
static int flag = 0;
static int reset = 0;
static int TIME = 0;
int  an = 90;
static float distanceL;
static float distanceR;

//引脚定义
const int IR_PIN[] = {A0, A1, A2, A3, 4, 6, 7, 8, 9, 10}; //红外对管引脚定义
const int IN_A1 = 11;   // 电机A1
const int IN_A2 = 12;  // 电机A2
const int IN_B1 = 5;  // 电机B1
const int IN_B2 = 2;  // 电机B2
LiquidCrystal lcd(49, 47 , 45 , 35 , 33 , 31 , 29 );
Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver(0x40);
void read_ir_values(void);
void read_HC_SR04_values(void);
void read_HC_SR04_values1(void);
void calculate_pid(void);
void motor_control(void);
void LCD_PRINT(void);
void LCD_PRINT1(void);
void ISRR1();
void ISRL1();
void FRONT1();
void KEY1();
void print_debug();

void setup()
{
  // 初始化
  pinMode(IN_A1, OUTPUT);
  pinMode(IN_A2, OUTPUT);
  pinMode(IN_B1, OUTPUT);
  pinMode(IN_B2, OUTPUT);
  pinMode(27,  INPUT);
  pinMode(25,  OUTPUT);
  pinMode(22, OUTPUT);
  pinMode(24, INPUT);
  pinMode(26, OUTPUT);
  pinMode(28, INPUT);
  pinMode(0, OUTPUT);
  pinMode(1, OUTPUT);
  for (int i = 0; i < 10; i++)
  {
    pinMode(IR_PIN[i], INPUT);
  }
  lcd.begin(16, 2);
  {
    Kp = analogRead(A15) / 10;
    Ki = analogRead(A13) / 10;
    Ki = Ki / 100;
    Kd = analogRead(A11) / 10;
    a = analogRead(A14) / 10;
    c = analogRead(A12) / 10;
    b = (a + c) / 2;
    initial_motor_speed = analogRead(A10) / 2;

  }
  attachInterrupt(5, ISRL1, FALLING); //开启外部中断2
  attachInterrupt(4, ISRR1, FALLING); //开启外部中断3
  attachIn