 bool yok           = false;
 bool zok           = false;
 
+long intX          = 0;
+long intY          = 0; 
+
+bool production    = false;
+bool LED_PING      = false;
+bool BOOT          = false;
+
+bool enMotAB       = false;
+bool enMotXY       = false;
+bool enMotZ        = false;
+
 unsigned long previousMillisX = 0;
 unsigned long previousMillisY = 0;
 unsigned long previousMillisZ = 0;

    {
    	Serial.readBytesUntil('\n', msg, sizeof msg);
    	newData = true;
	}
	
	if (radio.available())
    {
    	char msg[64] = {0};
    	radio.read(&msg, sizeof(msg));
    	newData = true;
    }
	
	if(newData)
	{
		Serial.println(msg);
		newData = false;
		
    	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, " "), "RESET") == 0)
    	{
    		Serial.println("RESET");
    	    production = false;
    	    emgStop    = false;
    	}
    	
    	if (strcmp(strtok(msg, " "), "BOOT") == 0)
    	{
    		
    	    BOOT = true;
    	    Serial.println("BOOTING UP...");
    	    delay(2000);

   
   if(BOOT)
   {
 	enMotAB = true;
 	enMotXY = true;
 	enMotZ  = true;
 	
   	// 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("BOOT 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("BOOT y1 ");
  		yok = true;
  		delay(2000);
  	    }
  	}
  		
  	// SET Z IN POSITION
  	if(!Z_POSITION)
  	{
  	    digitalWrite(pinStepZ, 1);
            delayMicroseconds( 500 );
    	    digitalWrite(pinStepZ, 0);
    	    delayMicroseconds( 500 );	
  	}
  	else
  	{
       	    if(!zok) 
  	    {
  	        Serial.println("BOOT z1 ");
  		zok = true;
  		Zangle = 0;
  		delay(2000);
  	    }	
  	}
  		
  	if( xok && yok && zok)
  	{
	    Serial.println("STAT b1 ");
  	    Serial.println("Booting UP OK!");
  	    Serial.println("SEND 'RUN \n' for start the production");
  	    BOOT = false;
  	}
     }
   
    // PRODUCTION CODE
    //-------------------------------------------------------------------------// 
    if(production && !emgStop)  //-
    {
    	enMotAB = true;
 	enMotXY = true;
 	enMotZ  = true;
    	
    	//======================================================================
    	//        d8888 
    	//       d88888 
     	//	d88P888 
    	//     d88P 888 
   	//    d88P  888 
  	//   d88P   888 
 	//  d8888888888 
	// d88P     888
	//======================================================================
    	// MOTOR A (Left)
    	
	// Run the motor
	if(speedA > 0) runA = true;
	else runA = false;
	speedA = 1000 / speedA;
		
	// Motor direction / With inversion
	if(invertA) 
  	{
    	    if(dirA) dirA = false;
	    else dirA = true;
	}
	digitalWrite(pinDirA, dirA); // Set the direction
		
	// Send Step inpultion to the motor
	unsigned long currentA = millis(); // millis();
	if (currentA - previousMillisA >= speedA) 
	{
	    previousMillisA = currentA;
	    
	    if(runA)
	    {
	        digitalWrite(pinStepA, 1);
	        delayMicroseconds( 500 );
	        digitalWrite(pinStepA, 0);
	        delayMicroseconds( 500 );
	    }
	} 
	  	
    	//======================================================================
	// 888888b.   
	// 888  "88b  
	// 888  .88P  
	// 8888888K.  
	// 888  "Y88b 
	// 888    888 
	// 888   d88P 
	// 8888888P"  
	//======================================================================	
	// MOTOR B (Right)
		
	// Run the motor
	if(speedB > 0) runB = true;
	else runB = false;
	speedB = 1000 / speedB;
	
	// Motor direction / With inversion
	if(invertB) 
 	{
	    if(dirB) dirB = false;
	    else dirB = true;
	}
	digitalWrite(pinDirB, dirB); // Set the direction
		
	// Send Step inpultion to the motor
	unsigned long currentB = millis(); // millis();
	if (currentB - previousMillisB >= speedB) 
	{
	    previousMillisB = currentB;
	      
	    if(runB)
	    {
	        digitalWrite(pinStepB, 1);
	        delayMicroseconds( 500 );
	        digitalWrite(pinStepB, 0);
	        delayMicroseconds( 500 );
	     }
	} 

    	//======================================================================
        // Y88b   d88P 
	//  Y88b d88P  
	//   Y88o88P   
	//    Y888P    
	//    d888b    
	//   d88888b   
	//  d88P Y88b  
	// d88P   Y88b  
	//======================================================================
  
	// 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
