Robot Test

//Lukas Klett - 2016
#include <NewPing.h>
#define Trigger_Pin1 3
#define Trigger_Pin2 5

#define Echo_Pin1  2  //Left Sensor
#define Echo_Pin2 4   //Right Sensor

#define MotorPin1 11 //Left Forward
#define MotorPin2 10 //Left Backwards
#define MotorPin3 9  //Right Forward
#define MotorPin4 6  //Right Backwards

#define Max_Distance 200

NewPing sonar1(Trigger_Pin1, Echo_Pin1, Max_Distance);
NewPing sonar2(Trigger_Pin2, Echo_Pin2, Max_Distance);

int number = 0;

void setup() {
  Serial.begin(115200);
  pinMode(MotorPin1, OUTPUT);
  pinMode(MotorPin2, OUTPUT);
  pinMode(MotorPin3, OUTPUT);
  pinMode(MotorPin4, OUTPUT);
}

void loop() {
  delay(50);

  unsigned int uS_1 = sonar1.ping();
  unsigned int uS_2 = sonar2.ping();

  int distance1 = sonar1.convert_cm(uS_1); //Left Distance
  int distance2 = sonar2.convert_cm(uS_2); //Right Distance

  /*Serial.print("Left distance to object: ");
  Serial.print(distance1);
  Serial.println("cm");

  Serial.print("Right distance to object: ");
  Serial.print(distance2);
  Serial.println("cm");*/

  if((distance1 <= 10 && distance2 <= 10)){  //Move Backwards
    analogWrite(MotorPin1, 0);
    analogWrite(MotorPin2, 255);
    analogWrite(MotorPin3, 0);
    analogWrite(MotorPin4, 255);
    number++;
    delay(1500);
    Serial.println("Moves Backwards");

    if((number % 2 == 0) && (distance1 > 10 && distance2 > 10)){
      analogWrite(MotorPin1, 255);
      analogWrite(MotorPin2, 0);
      analogWrite(MotorPin3, 0);
      analogWrite(MotorPin4, 255);
      delay(1000);
      Serial.println("Turns to the right");
    }
    if(number % 2 != 0 && (distance1 > 10 && distance2 > 10)){
      analogWrite(MotorPin1, 0);
      analogWrite(MotorPin2, 255);
      analogWrite(MotorPin3, 255);
      analogWrite(MotorPin4, 0);
      delay(1000);
      Serial.println("Turns to the left");
    }
  }

  if(number > 100){
    number == 0;
  }

  if(distance1 <= 10 && distance2 > 10){ //If Object appears on the left side dodge it to the right
    analogWrite(MotorPin1, 255);
    analogWrite(MotorPin2, 0);
    analogWrite(MotorPin3, 0);
    analogWrite(MotorPin4, 255);
    delay(1000);
    Serial.println("Dodges Object to the right");
  }

  if(distance2 <= 10 && distance1 > 10){ //If Object appears on the right side dodge it to the left
    analogWrite(MotorPin1, 0);
    analogWrite(MotorPin2, 255);
    analogWrite(MotorPin3, 255);
    analogWrite(MotorPin4, 0);
    delay(1000);
    Serial.println("Dodges Object to the left");
  }

  if(distance1 > 10 && distance2 > 10){
    analogWrite(MotorPin1, 255);
    analogWrite(MotorPin2, 0);
    analogWrite(MotorPin3, 255);
    analogWrite(MotorPin4, 0);
    Serial.println("Moves forward");
  }







}