//LUMIERE (LED)
//int ledPin = 13;                // LED connected to digital pin 13

//MOTEUR
const int AVANT = 10; // Constante pour la broche 10
const int ARRIERE = 9; // Constante pour la broche 9

int maxSpeed = 0;
int speedAmount = 5;

//SONAR
#define CM 1      //Centimeter
#define INC 0     //Inch
#define TP 7      //Trig_pin
#define EP 8      //Echo_pin

void stop() //Stop
{
  if (maxSpeed == 255) {
    speedAmount = -speedAmount ;
  }
}
void advance(char a) //Move forward
{
  analogWrite(AVANT, a);
  maxSpeed = maxSpeed + speedAmount;
}
void back_off (char a) //Move backward
{
  analogWrite(ARRIERE, a);
  maxSpeed = maxSpeed + speedAmount;
}

void setup(){
  //LUMIERE (LED)
  //pinMode(ledPin, OUTPUT);      // sets the digital pin as output

  //MOTEUR
  pinMode(9, OUTPUT);
  pinMode(10, OUTPUT);

  //SONAR
  pinMode(TP,OUTPUT);       // set TP output for trigger
  pinMode(EP,INPUT);        // set EP input for echo

  /*Serial.begin(9600);      // init serial 9600
  Serial.println("---------------------------------HC-SR04 Test---------------------------------");*/
}

void loop(){
  //SONAR
  long microseconds = TP_init();
  long distance_cm = Distance(microseconds, CM);
  /*Serial.print("Distance_CM = ");
  Serial.println(distance_cm);*/
  //delay(1000);

  if(distance_cm <= 10){
  /*  //LUMIERE
      digitalWrite(ledPin, HIGH);   // sets the LED on
      delay(50);                  // waits for a second
      digitalWrite(ledPin, LOW);    // sets the LED off
      delay(50);                  // waits for a second*/

   //MOTEUR
      /*stop();
      delay(1000);*/
      back_off (maxSpeed);
      delay(2000);
      /*stop();*/
    }
  else{
    //digitalWrite(ledPin, LOW);
    advance(maxSpeed);
    //delay(30);
  }
}
//SONAR
long Distance(long time, int flag)
{
  long distance;
  if(flag)
    distance = time /29 / 2  ;     // Distance_CM  = ((Duration of high level)*(Sonic :340m/s))/2
                                   //              = ((Duration of high level)*(Sonic :0.034 cm/us))/2
                                   //              = ((Duration of high level)/(Sonic :29.4 cm/us))/2
  else
    distance = time / 74 / 2;      // INC
  return distance;
}

//SONAR
long TP_init()
{
  digitalWrite(TP, LOW);
  delayMicroseconds(2);
  digitalWrite(TP, HIGH);                 // pull the Trig pin to high level for more than 10us impulse
  delayMicroseconds(10);
  digitalWrite(TP, LOW);
  long microseconds = pulseIn(EP,HIGH);   // waits for the pin to go HIGH, and returns the length of the pulse in microseconds
  return microseconds;                    // return microseconds
}