/*
 AVOIDING OBSTICLES WITH ULTRASONIC SENSOR 23.09.2019
 */
const int trigPin = 0;
const int echoPin = 1;

int led1 = 3 ;   // OUT 1
int led2 = 5 ;   // OUT 2
int mot2a = 6 ;  // OUT 3 (motor 2) PWM
int mot2b = 9 ;  // OUT 4 (motor 2) PWM
int mot1a = 10 ; // OUT 7 (motor 1) PWM
int mot1b = 11 ; // OUT 8 (motor 1) PWM

int brz = 96;
int svj = 16;
    // defines variables
long duration, cm, inches;
int broj;
void setup() {
  pinMode(trigPin, OUTPUT); // Sets the trigPin as an Output
  pinMode(echoPin, INPUT); // Sets the echoPin as an Input
  pinMode(led1, OUTPUT);
  pinMode(led2, OUTPUT); 
  pinMode(mot1a, OUTPUT);  
  pinMode(mot1b, OUTPUT);
  pinMode(mot2a, OUTPUT);  
  pinMode(mot2b, OUTPUT);    
  }
void loop() {
  // Clears the trigPin
  digitalWrite(trigPin, LOW);
  delayMicroseconds(5);
  // Sets the trigPin on HIGH state for 10 micro seconds
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  // Reads the echoPin, returns the sound wave travel time in microseconds
  pinMode(echoPin, INPUT);    
  duration = pulseIn(echoPin, HIGH);
  // Calculating the distance
  cm = (duration/2)/29.1;
  if (cm < 10) // obsticle detected
  { 
    digitalWrite(led1, svj); 
    digitalWrite(led2, 0);             
    analogWrite(mot1a, 0); // drive back
    analogWrite(mot1b, brz);
    analogWrite(mot2a, 0);
    analogWrite(mot2b, brz);
    delay(500);   
    digitalWrite(led1, 0);    
    broj = random(100);
    if (broj > 49)
    {    
      analogWrite(mot1a, brz); // turn left / random
      analogWrite(mot1b, 0);
      analogWrite(mot2a, 0);
      analogWrite(mot2b, brz);
    }
    else
    {
      analogWrite(mot1a, 0); // turn right / random
      analogWrite(mot1b, brz);
      analogWrite(mot2a, brz);
      analogWrite(mot2b, 0);    
    }
    delay(200);    
  }
  else
  {
    analogWrite(led2, svj);   
    analogWrite(mot1a, brz); // drive
    analogWrite(mot1b, 0);
    analogWrite(mot2a, brz);
    analogWrite(mot2b, 0); 
  }
}