#include <Servo.h>
// Pinii motor 1
#define mpin00 5
#define mpin01 6
// Pinii motor 2
#define mpin10 3
#define mpin11 11

const int trigPin = 2;
const int echoPin = 12;
// defines variables
long duration;
int distance1;


Servo srv;
void setup() {
 // configurarea pinilor motor ca iesire, initial valoare 0
 digitalWrite(mpin00, 0);
 digitalWrite(mpin01, 0);
 digitalWrite(mpin10, 0);
 digitalWrite(mpin11, 0);
 pinMode (mpin00, OUTPUT);
 pinMode (mpin01, OUTPUT);
 pinMode (mpin10, OUTPUT);
 pinMode (mpin11, OUTPUT);
 // pin LED
 pinMode(13, OUTPUT);

 pinMode(trigPin, OUTPUT); // Sets the trigPin as an Output
pinMode(echoPin, INPUT); // Sets the echoPin as an Input
Serial.begin(9600); // Starts the serial communication


}
// Functie pentru controlul unui motor
// Intrare: pinii m1 si m2, directia si viteza
void StartMotor (int m1, int m2, int forward, int speed)
{

 if (speed==0) // oprire
 {
 digitalWrite(m1, 0);
 digitalWrite(m2, 0);
 }
 else
 {
 if (forward)
 {
 digitalWrite(m2, 0);
 analogWrite (m1, speed); // folosire PWM
 }
 else
 {
 digitalWrite(m1, 0);
 analogWrite(m2, speed);
 }
 }
}
// Functie de siguranta
// Executa oprire motoare, urmata de delay
void delayStopped(int ms)
{
 StartMotor (mpin00, mpin01, 0, 0);
 StartMotor (mpin10, mpin11, 0, 0);
 delay(ms);
}
// Utilizare servo
// Pozitionare in trei unghiuri
// La final, ramane in mijloc (90 grade)
void playWithServo(int pin)
{
 srv.attach(pin);
 srv.write(0);
 delay(1000);
 srv.write(180);
 delay(1000);
 srv.write(90);
 delay(1000);
 srv.detach();
}
void loop() {

  digitalWrite(trigPin, LOW);
delayMicroseconds(2);
// 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
duration = pulseIn(echoPin, HIGH);
// Calculating the distance
distance1= duration/58.2;
// Prints the distance on the Serial Monitor
Serial.print("Distance: ");
Serial.println(distance1);


 // Cod avertizare
 // Blink lent
 for (int i=0; i<10; i++)
 {
 digitalWrite(13, 1);
 delay(200);
 digitalWrite(13, 0);
 delay(200);
 }
 // Blink rapid. Scoateti cablul USB!!!!
 for (int i=0; i<10; i++)
 {
 digitalWrite(13, 1);
 delay(100);
 digitalWrite(13, 0);
 delay(100);
 }
 digitalWrite(13, 1);
 // Pornirea motorului Servo
 //playWithServo(8);
 // Acum se pornesc motoarele DC

 if(distance1 <= 12){
 StartMotor (mpin00, mpin01, 1, 128);
 StartMotor (mpin10, mpin11, 1, 128);
 }
 else
 {
 StartMotor (mpin00, mpin01, 1, 0);
 StartMotor (mpin10, mpin11, 1, 0);
 }

 //delay (500); // Cat timp e motorul pornit
 //delayStopped(500); // Cat timp e oprit

// StartMotor (mpin00, mpin01, 1, 128);
 //StartMotor (mpin10, mpin11, 1, 128);

// delay (500);
// delayStopped(500);
// StartMotor (mpin00, mpin01, 0, 128);
// StartMotor (mpin10, mpin11, 1, 128);
//
// delay (500);
// delayStopped(500);
//
// StartMotor (mpin00, mpin01, 1, 128);
// StartMotor (mpin10, mpin11, 0, 128);
//
// delay (500);
// delayStopped(500);


// Clears the trigPin



}