Engelden kaçan robot

Arduino Kod:

#include <Servo.h>
#include <NewPing.h>

Servo myservo;

int ENABLE_A = 11;
int pin_a1 = 9;
int pin_a2 = 8;

int ENABLE_B = 5;
int pin_b1 = 6;
int pin_b2 = 7;

int SENSOR_DISTANCE;
int LEFT_SENSOR_DISTANCE;
int RIGHT_SENSOR_DISTANCE;
int LFT_SNZ_DIS;
int RGT_SNZ_DIS;
int rastgele;
#define TRIG_PIN 3
#define ECHO_PIN 2
#define MIN_DISTANCE 20
#define MAX_DISTANCE 100

NewPing sonar(TRIG_PIN, ECHO_PIN, MAX_DISTANCE);
 
void setup() {
 
  //Serial.begin(9600);
  //pin mode for the DC motors
  pinMode (ENABLE_A, OUTPUT);
  pinMode (pin_a1, OUTPUT);
  pinMode (pin_a2, OUTPUT);
 
  pinMode (ENABLE_B, OUTPUT);
  pinMode (pin_b1, OUTPUT);
  pinMode (pin_b2, OUTPUT);
 
  //pin mode for the ultrasonic sensor
  pinMode(TRIG_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);
 
  //for servo motor
  myservo.attach(10);
 
  sensorCenter();
 
}

void loop() {

  randomSeed(analogRead(0));

  SENSOR_DISTANCE=sensorDistance();
   //Serial.print("Front sensor distance is: ");
   //Serial.println(SENSOR_DISTANCE);
  
  if(SENSOR_DISTANCE >= MIN_DISTANCE || SENSOR_DISTANCE==0) {
   
    rastgele=random(25);
    if (rastgele>2){
      goForward();
    }
    else if (rastgele>1){
      goLeft();
      delay(333);
    }
    else if (rastgele>0){
      goRight();
      delay(333);
    }

  }
  else {
     
    //stop the motors
    stopMotors();

    LFT_SNZ_DIS=toTheLeft();
    RGT_SNZ_DIS=toTheRight();
   
    if (abs(RGT_SNZ_DIS-LFT_SNZ_DIS)<4 && LFT_SNZ_DIS<MIN_DISTANCE){
      goBackward();
      delay(500);
      stopMotors();
      sensorCenter();
    }
    else if(LFT_SNZ_DIS > RGT_SNZ_DIS) {
      goLeft();
      delay(450);
      stopMotors();
     
      sensorCenter();
    }
    else if(RGT_SNZ_DIS > LFT_SNZ_DIS) {
      goRight();
      delay(450);
      stopMotors();
 
      sensorCenter();
    }

  }

}

int sensorDistance(){
  delay(180);
  unsigned int distance=sonar.ping_cm();
  return distance;
}

void stopMotors(){
  analogWrite (ENABLE_A, 0);
  analogWrite (ENABLE_B, 0);
  digitalWrite (pin_a1, LOW);
  digitalWrite (pin_a2, LOW);
  digitalWrite (pin_b1, LOW);
  digitalWrite (pin_b2, LOW);
}

void goForward(){
  analogWrite (ENABLE_A, 180);
  digitalWrite (pin_a1, HIGH);
  digitalWrite (pin_a2, LOW);
  analogWrite (ENABLE_B, 180);
  digitalWrite (pin_b1, HIGH);
  digitalWrite (pin_b2, LOW);
}

void goBackward(){
  analogWrite (ENABLE_A, 180);
  digitalWrite (pin_a1, LOW);
  digitalWrite (pin_a2, HIGH);
  analogWrite (ENABLE_B, 180);
  digitalWrite (pin_b1, LOW);
  digitalWrite (pin_b2, HIGH);
}

void goRight(){
  analogWrite (ENABLE_A, 180);
  digitalWrite (pin_a1, HIGH);
  digitalWrite (pin_a2, LOW);
  analogWrite (ENABLE_B, 180);
  digitalWrite (pin_b1, LOW);
  digitalWrite (pin_b2, HIGH);
}

void goLeft(){
  analogWrite (ENABLE_A, 180);
  digitalWrite (pin_a1, LOW);
  digitalWrite (pin_a2, HIGH);
  analogWrite (ENABLE_B, 180);
  digitalWrite (pin_b1, HIGH);
  digitalWrite (pin_b2, LOW);
}

void sensorCenter(){
  myservo.write(90);
  delay(500);
}
 
int toTheLeft(){
  int LEFT_SENSOR_DISTANCE;
  myservo.write(120);
  delay(500);
  LEFT_SENSOR_DISTANCE=sensorDistance();
  //Serial.print("Left sensor distance is: ");
  //Serial.println(LEFT_SENSOR_DISTANCE);
  return LEFT_SENSOR_DISTANCE;
} 

int toTheRight(){
  int RIGHT_SENSOR_DISTANCE;
  myservo.write(60);
  delay(500);
  RIGHT_SENSOR_DISTANCE=sensorDistance();
  //Serial.print("Right sensor distance is: ");
  //Serial.println(RIGHT_SENSOR_DISTANCE);
  return RIGHT_SENSOR_DISTANCE;
}