 bool runY          = false; 
 bool directX       = false; // false = Gauche / true = droite
 bool directY       = false; // false = Gauche / true = droite
+
+bool xok           = false;
+bool yok           = false;
+bool zok           = false;
 
 // PID
 unsigned long previousMillisX = 0;

 ###########################################

 https://github.com/Gruniek/OPEN-BB-X
 Made by Daniel M/Gruniek/MOUS
 
 Compatible only with an Arduino Nano and the specific PCB.
 PCB Link : https://github.com/Gruniek/OPEN-BB-X/tree/master/Head/Stabiliser/PCB

    Serial.println("Testing device connections...");
    Serial.println(accelgyro.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");

}

//=============================================================================//
//   LOOP
//=============================================================================//
void loop()
{
    // GET MOTION
    //-------------------------------------------------------------------------//
    unsigned long currentG = millis(); // millis();

    if (currentG - trigG  >= trigGyro) 
    {
    	trigG = currentG;
      
      
    	accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
		if(invertXY)
		{
			y = (((ax/180)+etalonX)+90);
  			x = (((ay/180)+etalonY)+90);
		}
		else
		{
			x = (((ax/180)+etalonX)+90);
  			y = (((ay/180)+etalonY)+90);
		}
    }
  
    // GET SERIAL DATA
    //-------------------------------------------------------------------------//
	  
    char msg[30];
    if (Serial.available() > 0)
    {
    	Serial.readBytesUntil('\n', msg, sizeof msg);

    	if (strcmp(strtok(msg, " "), "SET") == 0)
    	{
        	Serial.println("UPDATE SETPOINT");
      		// trouvé le message
      		char *p;
      		while ((p = strtok(NULL, " ")) != NULL)
      		{
        		int val = atoi(p + 1);
        		switch (*p)
        		{
          			case 'x': setpointX  = val; break;
          			case 'y': setpointY  = val; break;
      	  			case 'm': multiplier = val; break;
      				case 'i': invertXY   = val; break;
      				case 'j': invertX    = val; break;
      				case 'k': invertX    = val; break;
      				case 'h': hysteresis = val; break;
        		}
      		}
    	}
    
    	if (strcmp(strtok(msg, " "), "RUN") == 0)
    	{
        	Serial.println("ROll BB-8 ROLL !");
      	    production = true;
    	}
    
    	if (strcmp(strtok(msg, " "), "STOP") == 0)
    	{
    		Serial.println("STOP");
    	    production = false;
    	}
    	
    	if (strcmp(strtok(msg, " "), "BOOT") == 0)
    	{
    		Serial.println("STOP");
    	    BOOT = true;
    	    Serial.println("BOOTING UP...");
    	    delay(2000);
    	}
    	
    	
  }
   
   if(boot)
   {
   	    // SET X IN POSITION
   		if(x < 90) directX = false;
  		else directX = true;
  
  		if(invertX) 
  		{
    		if(directX) directX = false;
    		else directX = true;
  		}
  		digitalWrite(pinDirX, directX); // dirX
  		
  		if(x > (setpointX + hysteresis) || (x < setpointX - hysteresis ))
  		{
  	        digitalWrite(pinStepX, 1);
        	delayMicroseconds( 500 );
    	    digitalWrite(pinStepX, 0);
    	    delayMicroseconds( 500 );
  		}
  		else
  		{
  			if(!xok) 
  			{
  				Serial.println(STAT x1);
  				xok = true;
  				delay(2000);
  			}
  		}
  		
  		
  		// SET Y IN POSITION
   		if(y < 90) directY = false;
  		else directY = true;
  
  		if(invertY) 
  		{
    		if(directY) directY = false;
    		else directY = true;
  		}
  		digitalWrite(pinDirY, directY); // dirX
  		
  		if(x > (setpointY + hysteresis) || (y < setpointY - hysteresis ))
  		{
  	        digitalWrite(pinStepY, 1);
        	delayMicroseconds( 500 );
    	    digitalWrite(pinStepY, 0);
    	    delayMicroseconds( 500 );
  		}
  		else
  		{
  			if(!yok) 
  			{
  				Serial.println(STAT y1);
  				yok = true;
  				delay(2000);
  			}
  		}
  		
  		// SET W IN POSITION
  		
  		if()
  
  
   }
   
	// PRODUCTION CODE
    //-------------------------------------------------------------------------// 
    if(production)  //-
    { 

  // X //
  
  // Direction
  if(x < 90) directX = false;
  else directX = true;
  
  if(invertX) 
  {
    if(directX) directX = false;
    else directX = true;
  }
  
  digitalWrite(pinDirX, directX); // dirX
  
  // Si ont doit demarer le moteur
  if(x > (setpointX + hysteresis) || (x < setpointX - hysteresis )) 
