Arduino机器人汽车（四轮驱动）的编程避免障碍物不会遵循我们的自定义MoveSet

Question

我正在尝试为我的Arduino机器人车编程例程，以便它可以浏览简单的障碍物。目的是进行直线跟踪，最多可达17“ x12”框并绕过它。我无法找到一种巧妙地在盒子上移动的方法在盒子里，向右移动几秒钟，向前移动几秒钟，向左移动几秒钟，向前移动，然后向左手动移动到盒子上）。由于某种原因，机器人会停在盒子上，向左转，然后停下来。我相信它从编程中旋转了，因为不再有任何线路要遵循并感到困惑。任何帮助都将不胜感激，这是我对编程的介绍，而我非常迷失。对于代码中的许多评论，我不得不逐行了解发生了什么，这就是我审查实际发生的事情的方式。

[代码]

// Include the Servo library
#include <Servo.h>
// Define object 'myservo' to control the sensor
Servo myservo;

//The pin CONSTANTS for the line tracking sensors
#define LT_R !digitalRead(10)
#define LT_M !digitalRead(4)
#define LT_L !digitalRead(2)

// The pins CONSTANTS for other stuff
#define ENA 5
#define ENB 6
#define IN1 7
#define IN2 8
#define IN3 9
#define IN4 11

// A constant for our car speed
#define carSpeed 130

// Define variables to store the value of the sensor readings
int Echo = A4;  
int Trig = A5; 
// Define variables to hold our right, left, and middle distance values
int rightDistance = 0, leftDistance = 0, middleDistance = 0;

// Function to go forward
void forward(){
  analogWrite(ENA, carSpeed);
  analogWrite(ENB, carSpeed);
  digitalWrite(IN1, HIGH);
  digitalWrite(IN2, LOW);
  digitalWrite(IN3, LOW);
  digitalWrite(IN4, HIGH);
  Serial.println("go forward!");
}
void forward(int delayTime){
  int secDelay = delayTime*1000;
  analogWrite(ENA, carSpeed);
  analogWrite(ENB, carSpeed);
  digitalWrite(IN1, HIGH);
  digitalWrite(IN2, LOW);
  digitalWrite(IN3, LOW);
  digitalWrite(IN4, HIGH);
  Serial.println("go forward!");
  delay(secDelay);
}

// Function to go backward
void back(){
  analogWrite(ENA, carSpeed);
  analogWrite(ENB, carSpeed);
  digitalWrite(IN1, LOW);
  digitalWrite(IN2, HIGH);
  digitalWrite(IN3, HIGH);
  digitalWrite(IN4, LOW);
  Serial.println("go back!");
}

// Function to go left
void left(){
  analogWrite(ENA, carSpeed);
  analogWrite(ENB, carSpeed);
  digitalWrite(IN1, LOW);
  digitalWrite(IN2, HIGH);
  digitalWrite(IN3, LOW);
  digitalWrite(IN4, HIGH);
  Serial.println("go left!");
}
void left(int turnAngle){
  int turnTime = 4*turnAngle;
  analogWrite(ENA, carSpeed);
  analogWrite(ENB, carSpeed);
  digitalWrite(IN1, LOW);
  digitalWrite(IN2, HIGH);
  digitalWrite(IN3, LOW);
  digitalWrite(IN4, HIGH);
  Serial.println("go left!");
  delay(turnTime);
}

// Function to go right
void right(){
  analogWrite(ENA, carSpeed);
  analogWrite(ENB, carSpeed);
  digitalWrite(IN1, HIGH);
  digitalWrite(IN2, LOW);
  digitalWrite(IN3, HIGH);
  digitalWrite(IN4, LOW); 
  Serial.println("go right!");
} 
void right(int turnAngle){
  int turnTime = 4*turnAngle;
  analogWrite(ENA, carSpeed);
  analogWrite(ENB, carSpeed);
  digitalWrite(IN1, HIGH);
  digitalWrite(IN2, LOW);
  digitalWrite(IN3, HIGH);
  digitalWrite(IN4, LOW); 
  Serial.println("go right!");
  delay(turnTime);
} 


// Function to stop
void stop(){
   digitalWrite(ENA, LOW);
   digitalWrite(ENB, LOW);
   Serial.println("Stop!");
} 

// Function to measure distance with the ultrasonic sensor
int Distance_test() {
  digitalWrite(Trig, LOW);   
  delayMicroseconds(2);
  digitalWrite(Trig, HIGH);  
  delayMicroseconds(20);
  digitalWrite(Trig, LOW);   
  float Fdistance = pulseIn(Echo, HIGH);  
  Fdistance= Fdistance / 60;       
  return (int)Fdistance;
}  
void avoidObject(){
  // Take a reading of what is in front of us  
  middleDistance = Distance_test();
  // Print the result in the Serial monitor
  Serial.print("Middle Distance = ");
  Serial.println(middleDistance);

  // If the distance to an obstacle is less than 20
  if(middleDistance <= 20) {
    int lastTurnLeft = 0;     
    // STOP!
    stop();
    // Wait half a second
    delay(500);
    // Now go in reverse                         
    //back();
    // And set the servo to point right
    myservo.write(0);
    // Wait a second...
    delay(200);
    // stop.
    stop();
    
    //  Read the distance to any obstacle that may be on the right      
    rightDistance = Distance_test();
    // Print the right distance on the serial monitor
    Serial.print("Right Distance = ");
    Serial.println(rightDistance);
    // Wait half a second
    delay(500);
    // Return the ultrasound sensor to the middle position
    myservo.write(90);
    // Wait another half a second to give the sensor a chance to catch up              
    delay(500);
    // And now set the servo to point to the left                                                  
    myservo.write(180);
    // Wait another half a second so it can catch up.              
    delay(500);
    // Take a reading to any obstacles that may be on the left
    leftDistance = Distance_test();
    // Print the left distance on the Serial monitor
    Serial.print("Left Distance = ");
    Serial.println(leftDistance);
    // Wait half a second
    delay(500);
    // Return the ultrasound sensor to the middle position
    myservo.write(90);
    // And wait a second              
    delay(1000);
    // Now, if the obstacle on the left is close to us
    if(rightDistance > leftDistance) {
      // Then let's go right
      right();
      delay(1000);
      stop();
     
    }
    // Else if the obstacle on the right is closer to us
    else if(rightDistance < leftDistance) {
      // Then let's go left
      left();
      delay(1000);
      stop();
     
    }
    // Break out of the object avoidance code
  }
}
bool isColliding(){
  int dist = Distance_test();
  if(dist <= 20){
    return true;
  }
  else{
    return false;
  }
}

// Function to Initialize the Arduino
void setup(){
  // Attach the servo to pin 3
  myservo.attach(3);
  // Start the Serial Monitor
  Serial.begin(9600);     
  // Define the mode of our pins
  pinMode(Echo, INPUT);    
  pinMode(Trig, OUTPUT);  
  pinMode(IN1, OUTPUT);
  pinMode(IN2, OUTPUT);
  pinMode(IN3, OUTPUT);
  pinMode(IN4, OUTPUT);
  pinMode(ENA, OUTPUT);
  pinMode(ENB, OUTPUT);
  pinMode(LT_R,INPUT);
  pinMode(LT_M,INPUT);
  pinMode(LT_L,INPUT);
  // Stop the car
  stop();
  // Put the sensor in the middle position 
  myservo.write(90);
}



    //At this point we should have found a line
    // Simply let the function return and
    // let loop() function handle the line tracking agai`enter code here`n
    enter code here

void loop() {
  int colCount = 0;
  // This is where the loop begins
  bool collision = isColliding();
  // LINE FOLLOWER
  while((LT_M||LT_R||LT_L)&&(collision==false)){
  // If we detect the middle sensor
    if(LT_M){
      // Then go forward
      forward();
    }
    // Else if we detect the RIGHT sensor
    else if(LT_R) {
      // Then go right
      right();
      // And keep going right until we don't detect anything on the right sensor
      while(LT_R);                             
    }
    // Else if we detect something on the left sensor
    else if(LT_L) {
      // Then go left
      left();
      // And keep going left until we don't detect anything on the left sensor
      while(LT_L);  
    }
    avoidObject();
  }  
  if (collision == true){
   colCount==1;
    while (colCount==1){
    right(90);
    forward(5);
    left(90);
    forward(10);
    left(90); 
    forward(5);
    right(90);  
    colCount=colCount+1;
    }
    
    
  }
  else{  
    
   
   stop();
  }
  
  
  // OBJECT AVOIDANCE
  // 17" x 12" box

  
// Go back to the start of the loop                     
}

// What happens: line tracks fine, stops at box fine, once it turns it will not go, professor suggests its detecting the wall so turn right instead?

[/代码]