#include <Pixy2.h> 
               
Pixy2 pixy;

         //PixyCam
int signature = 0; //initialisation couleur
int x = 0; //position sur x
int y = 0; //position sur y
int width = 0; //largeur=0
int height = 0; //hauteur=0
int Xmin = 95; //limite Xmin au pixel 95
int Xmax = 200; //limite Xmax au pixel 200


         //moteurs
int M1 = 5;  //Moteur 1
int M2 = 10;  //Moteur 2
int M3 = 9;  //Moteur 3
int M4 = 6;  //Moteur 4

         // Ultrason
int TRIG = 3; //borne trigger de l'ultrason
int ECHO = 2; //borne echo de l'ultrason
long lecture_echo;
long cm;

void setup()
{
Serial.begin(115200); //nombre de baud pour le moniteur série
pixy.init(); //initialisation Pixy

pinMode (M1, OUTPUT); //déclare que M1 est une sortie
pinMode (M2, OUTPUT); //déclare que M2 est une sortie
pinMode (M3, OUTPUT); //déclare que M3 est une sortie
pinMode (M4, OUTPUT); //déclare que M4 est une sortie
pinMode (TRIG, OUTPUT);//Emetteur ultrasons
digitalWrite(TRIG, LOW);
pinMode (ECHO, INPUT); //Récepteur ultrasons

}

void avancer()
{

analogWrite (M1, 50); //Fait tourner le moteur à 95% de sa vitesse
analogWrite (M2, 50); //Fait tourner le moteur à 95% de sa vitesse
analogWrite (M3, 50); //Fait tourner le moteur à 95% de sa vitesse
analogWrite (M4, 50); //Fait tourner le moteur à 95% de sa vitesse

}

void droite()
{

analogWrite (M1, 150); //Fait tourner le moteur à 95% de sa vitesse
analogWrite (M2, 150); //Fait tourner le moteur à 95% de sa vitesse
analogWrite (M3, 15); //Fait tourner le moteur à 15% de sa vitesse
analogWrite (M4, 15); //Fait tourner le moteur à 15% de sa vitesse

}

void gauche()
{
analogWrite (M1, 15); //Fait tourner le moteur à 15% de sa vitesse
analogWrite (M2, 15); //Fait tourner le moteur à 15% de sa vitesse
analogWrite (M3, 150); //Fait tourner le moteur à 95% de sa vitesse
analogWrite (M4, 150); //Fait tourner le moteur à 95% de sa vitesse

}

void stop()
{
  
analogWrite (M1, 0); //Arrête le moteur
analogWrite (M2, 0); //Arrête le moteur
analogWrite (M3, 0); //Arrête le moteur
analogWrite (M4, 0); //Arrête le moteur
}

void loop()
{
 

  digitalWrite(TRIG, HIGH);  
  delayMicroseconds(10); 
  digitalWrite(TRIG, LOW);
  lecture_echo = pulseIn(ECHO, HIGH);
  cm = lecture_echo /58;
  Serial.print("Distance en cm :");
  Serial.println(cm);

if (cm <= 30);
{
   Serial.println("Arret d'urgence");
   stop();
}
{
  
int i;
pixy.ccc.getBlocks();
if (pixy.ccc.numBlocks)
{
Serial.println(pixy.ccc.numBlocks);
for (i=0; i<pixy.ccc.numBlocks; i++)
{
Serial.print("block");
Serial.print(i);
Serial.print(":");

  pixy.ccc.blocks[i].print();  
  x = pixy.ccc.blocks[i].m_x;          
  y = pixy.ccc.blocks[i].m_y;                      
  width = pixy.ccc.blocks[i].m_width;              
  height = pixy.ccc.blocks[i].m_height;            
  signature = pixy.ccc.blocks[i].m_signature;     
  
  
if(signature == 1) //s’il reconnait l’objet
{
if (x < Xmin) //si x < Xmin
{
gauche();
}

else if (x > Xmax) //sinon si x > Xmax
{
droite(); //faire fonction droite
}

else if (x < Xmax) //sinon si x < Xmax
{
avancer(); //faire fonction avncer
}

else //sinon
{
stop(); //fonction stop
}
}
else //sinon
{
stop(); //fonction stop
}
}
}
}
}