这里我把第二张原理图中的两个红外反射传感器换成了超声波测距，一共三个超声波测距，a0-a5口都占满了：

上代码：

#include <AFMotor.h>

#define TRIG_L A0
#define ECHO_L A1

#define TRIG_M A2
#define ECHO_M A3

#define TRIG_R A4
#define ECHO_R A5

int distance_l;
int distance_r;
int distance_m;

int i;

AF_DCMotor left_rear(1 );
AF_DCMotor left_front(2);
AF_DCMotor right_rear(3);
AF_DCMotor right_front(4);

int SPEED = 110; //0-255

void setup() {
  Serial.begin(9600);

  pinMode(ECHO_L, INPUT);                    //Set EchoPin as input, to receive measure result from US-025,US-026
  pinMode(TRIG_L, OUTPUT);

  pinMode(ECHO_R, INPUT);                    //Set EchoPin as input, to receive measure result from US-025,US-026
  pinMode(TRIG_R, OUTPUT);

  pinMode(ECHO_M, INPUT);
  pinMode(TRIG_M, OUTPUT);
}

void loop() {
  delay(50);
  distance_l = get_left_distance();
  delay(50);
  distance_r = get_right_distance();
  delay(50);
  distance_m = get_mid_distance();


  if ((distance_l>250 && distance_l<5608)&&(distance_m>250 && distance_m<5608)&&(distance_r>250 && distance_r<5608)) {
    //1, 1, 1 继续前进
  Forward();
  }

  else if (  ( (distance_l >= 5608) || ( (distance_l >1) && (distance_l < 250)) )   &&   ( (distance_m >= 5608) || ( (distance_m > 0) && (distance_m < 250)) )   &&   ( (distance_r >= 5608) || ( (distance_r > 0) && (distance_r < 250))  )  ) {
    //0, 0, 0 停止前进，倒退默认向右转
    Serial.println("backward");
    Backward();
    if ( distance_r  > distance_l ) {
      TurnRight();
    }
    else {
      TurnLeft();
    }

  }
  else if (  ( (distance_m == 5609) || ( (distance_m > 0) && (distance_m < 250))   ||  (distance_m == 5608)  ) &&  ((distance_l < 5608) && (distance_l >= 250)) &&  ((distance_r < 5608) && (distance_r >= 250)) ) {
    //1, 0, 1中间有障碍， 后退并选择方向转一步
    Backward();
    if ( distance_r > distance_l  ) {
      TurnRight();
    }
    else {
      TurnLeft();
    }

  }

  else if ( (((distance_l > 0) && (distance_l < 250)) || (distance_l == 5609)   ||  (distance_l == 5608) )   &&   ( ((distance_m >= 250 ) && (distance_m < 5608))  )    &&    ((distance_r < 5608) &&  (distance_r >= 250)) ) {
    //0,1,1左边有障碍，直接往右转，然后前进
    Backward();
    TurnRight();

  }

  else if ( (((distance_l > 0) && (distance_l < 250)) || (distance_l == 5609)   ||  (distance_l == 5608) )   &&   ( ((distance_m > 0) && (distance_m < 250)) ||   (distance_m == 5609)  ||  (distance_m == 5608))    &&    ((distance_r < 5608) &&  (distance_r >= 250)) ) {
    //0,0,1左边和中间有障碍，后退并右转
    Backward();
    TurnRight();
  }

  else  if ( (((distance_r > 0) && (distance_r < 250)) || (distance_r == 5609)    ||  (distance_r == 5608) )   &&  ( ((distance_m >= 250 ) && (distance_m < 5608))  )   &&   ((distance_l < 5608) && (distance_l >= 250))) {
    //1,1,0 右边有障碍, 左转并前进
    Backward();
    TurnLeft();
  }

  else  if ( (((distance_r > 0) && (distance_r < 250)) || (distance_r >= 5608)  )   &&    ( ((distance_m > 0) && (distance_m < 250)) ||   (distance_m >= 5608) )    &&    ((distance_l < 5608) && (distance_l >= 250))  ) {
    //1, 0, 0右边和中间有障碍, 后退并左转
    Backward();
    TurnLeft();

  }

  else if (  ( (distance_l >= 5608) || ( (distance_l > 0) && (distance_l < 250)) )   &&   (  (distance_m > 250) && (distance_m < 5608) )   &&   ( (distance_r >= 5608) || ( (distance_r > 0) && (distance_r < 250))  )  ) {
    //0, 1, 0  继续前进
    Forward();
  }


}

unsigned long get_mid_distance() {
  unsigned long Time_Echo_us = 0;
  unsigned long Len_mm  = 0;

  digitalWrite(TRIG_M, HIGH);              //begin to send a high pulse, then US-025/US-026 begin to measure the distance
  delayMicroseconds(20);                    //set this high pulse width as 20us (>10us)
  digitalWrite(TRIG_M, LOW);               //end this high pulse

  Time_Echo_us = pulseIn(ECHO_M, HIGH);               //calculate the pulse width at EchoPin,
  if ((Time_Echo_us < 60000) && (Time_Echo_us > 1))    //a valid pulse width should be between (1, 60000).
  {
    Len_mm = (Time_Echo_us * 34 / 100) / 2; //calculate the distance by pulse width, Len_mm = (Time_Echo_us * 0.34mm/us) / 2 (mm)
    Serial.print("Present M_Distance is: ");  //output result to Serial monitor
    //Serial.print(Len_mm, DEC);            //output result to Serial monitor
    Serial.print(Len_mm);
    Serial.println("mm");                 //output result to Serial monitor
    return Len_mm;
  }
  else {
    return 0;
  }
}

unsigned long get_left_distance() {
  unsigned long Time_Echo_us = 0;
  unsigned long Len_mm  = 0;

  digitalWrite(TRIG_L, HIGH);              //begin to send a high pulse, then US-025/US-026 begin to measure the distance
  delayMicroseconds(20);                    //set this high pulse width as 20us (>10us)
  digitalWrite(TRIG_L, LOW);               //end this high pulse

  Time_Echo_us = pulseIn(ECHO_L, HIGH);               //calculate the pulse width at EchoPin,
  if ((Time_Echo_us < 60000) && (Time_Echo_us > 1))    //a valid pulse width should be between (1, 60000).
  {
    Len_mm = (Time_Echo_us * 34 / 100) / 2; //calculate the distance by pulse width, Len_mm = (Time_Echo_us * 0.34mm/us) / 2 (mm)
    Serial.print("Present L_Distance is: ");  //output result to Serial monitor
    //Serial.print(Len_mm, DEC);            //output result to Serial monitor
    Serial.print(Len_mm);
    Serial.println("mm");                 //output result to Serial monitor
    return Len_mm;
  }
  else {
    return 0;
  }
}

unsigned long get_right_distance() {
  unsigned long Time_Echo_us = 0;
  unsigned long Len_mm  = 0;

  digitalWrite(TRIG_R, HIGH);              //begin to send a high pulse, then US-025/US-026 begin to measure the distance
  delayMicroseconds(20);                    //set this high pulse width as 20us (>10us)
  digitalWrite(TRIG_R, LOW);               //end this high pulse

  Time_Echo_us = pulseIn(ECHO_R, HIGH);               //calculate the pulse width at EchoPin,
  if ((Time_Echo_us < 60000) && (Time_Echo_us > 1))    //a valid pulse width should be between (1, 60000).
  {
    Len_mm = (Time_Echo_us * 34 / 100) / 2; //calculate the distance by pulse width, Len_mm = (Time_Echo_us * 0.34mm/us) / 2 (mm)
    Serial.print("Present R_Distance is: ");  //output result to Serial monitor
    //Serial.print(Len_mm, DEC);            //output result to Serial monitor
    Serial.print(Len_mm);
    Serial.println("mm");                 //output result to Serial monitor
    return Len_mm;
  }
  else {
    return 0;
  }
}

void Stop() {
  left_front.setSpeed(0);
  left_rear.setSpeed(0);
  right_front.setSpeed(0);
  right_rear.setSpeed(0);

  left_front.run(RELEASE); 
  right_rear.run(RELEASE); 
  right_front.run(RELEASE); 
  left_rear.run(RELEASE); 
}



// function to move forward
void Forward() {
  Serial.println("forward is process");
  left_front.setSpeed(SPEED);
  left_rear.setSpeed(SPEED);
  right_front.setSpeed(SPEED);
  right_rear.setSpeed(SPEED);
  left_front.run(FORWARD); 
  right_rear.run(FORWARD); 
  right_front.run(FORWARD); 
  left_rear.run(FORWARD); 
}


// function to move backward
void Backward() {
  for (i = 600; i > 0; i--) {
    left_front.setSpeed(SPEED);
    left_rear.setSpeed(SPEED);
    right_front.setSpeed(SPEED);
    right_rear.setSpeed(SPEED);
    left_front.run(BACKWARD); 
    right_rear.run(BACKWARD);
    right_front.run(BACKWARD); 
    left_rear.run(BACKWARD); 
  }
}


// function to turn right
void TurnRight() {
  for (i = 500; i > 0; i--) {
    left_front.setSpeed(SPEED);
    left_rear.setSpeed(SPEED);
    right_front.setSpeed(SPEED);
    right_rear.setSpeed(SPEED);
    left_front.run(FORWARD); 
    right_rear.run(BACKWARD); 
    right_front.run(BACKWARD); 
    left_rear.run(FORWARD); 
  }
  /*
    digitalWrite(DIR_L, HIGH);
    digitalWrite(DIR_R, LOW);
    analogWrite(PWM_L, SPEED);
    analogWrite(PWM_R, SPEED);
    delay(200);
    analogWrite(PWM_L, 0);
    analogWrite(PWM_R, 0);
    delay(300);*/
}


// function to turn left
void TurnLeft() {
  for (i = 300; i > 0; i--) {
    left_front.setSpeed(SPEED);
    left_rear.setSpeed(SPEED);
    right_front.setSpeed(SPEED);
    right_rear.setSpeed(SPEED);
    left_front.run(BACKWARD); 
    right_rear.run(FORWARD);
    right_front.run(FORWARD); 
    left_rear.run(BACKWARD); 
  }
  /*
    digitalWrite(DIR_L, LOW);
    digitalWrite(DIR_R, HIGH);
    analogWrite(PWM_L, SPEED);
    analogWrite(PWM_R, SPEED);
    delay(200);
    analogWrite(PWM_L, 0);
    analogWrite(PWM_R, 0);
    delay(300);
  */
}