 bool ifPing        = false;
 bool emgStop       = false;
 bool production    = false;
+
+int setpointX      = 0;
+int setpointY      = 0;
+int multiplier     = 0;
+int hysteresis     = 0;
+bool invertXY      = 0;
+bool invertX       = 0;
+bool invertY       = 0;
+
+unsigned long currentA = 0;
+unsigned long previousMillisA = 0;
+unsigned long currentB = 0;
+unsigned long previousMillisB = 0;
 
 unsigned long trigSec  = 1000;
 unsigned long trigS    = 0;

    pinMode( PING        , OUTPUT );

	Serial.begin(9600);
	Serial.println("Booting up...");
    Serial.println(" ");
    Serial.println("_ Astromech Industrie _");
    Serial.print("  BB-8 Version ");
    Serial.println(version);
    Serial.println("===================");
    Serial.println(" ");
    


    
    Serial.print("Initializing I2C devices...  I2C ADRESS :");
    Serial.print(i2cAdress);
  
    Wire.begin(i2cAdress);
    
    accelgyro.initialize();
	Serial.println(" [ OK ]");

    Serial.print("Connect to the MPU6050 : ");
    Serial.println(accelgyro.testConnection());
    //Serial.println(accelgyro.testConnection() ? "Connection successful" : "Connection failed");
    
    Serial.println("Initializing RF24 devices...  ");
    radio.begin();
    radio.openWritingPipe(pipe);
    radio.startListening();
    
    /* Start I2C bus and PCF8574 instance */
  	expander.begin(0x20);
  
  	/* Setup some PCF8574 pins for demo */
  	expander.pinMode(0, OUTPUT);
  	expander.pinMode(1, OUTPUT);

    			production = true;
    		}
    	}
    	
    	
    	// SEND RF24 //
		String theMessage = "Hello there!";
		int messageSize = theMessage.length();
		for (int i = 0; i < messageSize; i++) 
		{
    		int charToSend[1];
    		charToSend[0] = theMessage.charAt(i);
    		radio.write(charToSend,1);
  		}  
 
		msg[0] = 2; 
    	radio.write(msg,1);
    
    }
  
}
