/*
 * Credits: Optimus Digital
 * Prezentare: Un scurt cod creat pentru kit-ul robot pentru ocolit obstacolele.
 * Atunci cand unul din senzori detectează un obiect, robotul se intoarce inapoi
 * si face un scurt viraj la stanga, dupa care isi continua miscarea inainte.
 * 
 * Este posibil ca viteza motoarelor sa difere. Pentru a corecta acest lucru, fie utilizati un
 * encoder pentru a ajusta precis viteza acestora, sau sa modificati factorul de umplere in urma 
 * testarii robotului.
 * 
 * Factorul de umplere este transmit ca parametru la functia analogWrite( pin, valoare). Pin-ul trebuie
 * sa poata sa transmita semnal de tip PWM ( consultați datasheet-ul placii de dezvoltare pentru a 
 * determina pinii compatibili.
 * 
 * 
 */
//definiți pinii utilizați de senzorii ultrasonici ( HC-SR04 )
#define echoPin1 10
#define trigPin1 9

#define echoPin2 2
#define trigPin2 13

//definiti viteza motoarelor
//modificați valoarea în caz ca motoarele nu se invart la aceeasi viteza
#define vitezaMotor1 80
#define vitezaMotor2 140

//definiti pinii utilizați pentru controlul motoarelor
//pinii enA si enB sunt folositi pentru a controla viteza motoarelor in functie
//de semnalul PWM
//pinii in1, in2, in3 si in4 sunt utilizati pentru a controla directia motoarelor(inainte sau inapoi) 
//pinii folositi de primul motor
#define enA 11
#define in1 7
#define in2 6

//pinii folositi de al doilea motor
#define enB 3
#define in3 5
#define in4 4

//
#define inainte HIGH
#define inapoi LOW

//declarați variabilele utilizate pentru a determina distanta obstacolelor din fata robotului
long duration1,duration2;
int distance1,distance2;

void directie (int dir1, int dir2)
{
 //daca dir este HIGH, directia este inainte
 //daca dir este LOW, directia este inapoi

 //motor 1
 digitalWrite(in1, !dir1);
 digitalWrite(in2, dir1);

 //motor 2
 digitalWrite(in3, dir2);
 digitalWrite(in4, !dir2);
 
 
}

void viteza (int vit1, int vit2)
{
  analogWrite(enA,vit1);
  analogWrite(enB,vit2);
}

void setup() {

//pinii utilizați pentru a emite ultrasonice sunt declarati ca output
//iar pinii utilizati pentru a recepta sunetul sunt declarati ca input

//pinii utilizati de primul senzor ultrasonic 
pinMode(trigPin1, OUTPUT); 
pinMode(echoPin1, INPUT); 

//pinii utilizati de al doilea senzor ultrasonic
pinMode(trigPin2, OUTPUT); 
pinMode(echoPin2, INPUT); 

//toti pinii utilizati de driverul L298N sunt OUTPUT
//pinii utilizati de primul motor 
pinMode(enA,OUTPUT); 
pinMode(in1,OUTPUT);
pinMode(in2,OUTPUT);

//pinii utilizati de al doilea motor 
pinMode(enB,OUTPUT);
pinMode(in3,OUTPUT);
pinMode(in4,OUTPUT);



//setati directia motoare pe inainte
//primul paramatru reprezinta directia primului motor
//iar cel de-al doilea parametru reprezinta directia celui de-al doilea motor
directie(inainte, inainte);

//setati viteza motoarelor la 0
viteza(0,0);
delay(5000);

}

void loop() {

//setati viteza motoarelor
viteza(vitezaMotor1, vitezaMotor2);

directie(inainte, inainte);

//pentru a masura distanta cu un senzor ultrasonic trebuie trimis un sunet
//dupa care se asteapta sa se intoarca ecoul la modul
//in functie de durata de timp se poate calcula distanta fata de un obiect 

//calculați distanta fata de primul senzor.
digitalWrite(trigPin1, LOW);
delayMicroseconds(2);
digitalWrite(trigPin1, HIGH);
delayMicroseconds(10);
digitalWrite(trigPin1, LOW);
duration1 = pulseIn(echoPin1, HIGH);
distance1= duration1*0.034/2;


//calculați distanta fata de al doilea senzor
digitalWrite(trigPin2, LOW);
delayMicroseconds(2);
digitalWrite(trigPin2, HIGH);
delayMicroseconds(10);
digitalWrite(trigPin2, LOW);
duration2 = pulseIn(echoPin2, HIGH);
distance2= duration2*0.034/2;


//daca distanta masurata de unul dintre senzori este mai mica de 20 de cm
//atunci robotul se va intoare si va efectua un viraj la stanga
//dupa care isi va continua miscarea inainte pana intalneste alt obstacol
//pentru a ajusta miscarile robotului, puteti sa modificati parametru functiei delay

if(distance1 < 30 || distance2 < 30 ) 
  { 
    
    directie(inapoi,inapoi);
    viteza(0,0);

    delay(1000);
    
    viteza(vitezaMotor1,vitezaMotor2);

    delay(1000);
    
    directie(inainte,inapoi);
    
    viteza(vitezaMotor1,vitezaMotor2);
    delay(300);
    
  }

delay(10);
}