  - ADD BOOT, STAT, ROTATE command
  - ADD Z motor configuration and production
  - ADD BOOT mode, for initilize or reinizialize the Droid position
- 
+
  10/2016
  ==========
  - Add PID for X and Y
- 
- 
+
+
  To do list
  ==========
  - Add Z rotation controller
- - Add Rx/Tx from the motherboard for change de coordinate of the head, 
+ - Add Rx/Tx from the motherboard for change de coordinate of the head,
   rotate the head and report to the remote the status off all sensors/positiom.
-  
-  
+
+
   Big thanks for http://r2builders.fr/ !
   Initial project : http://r2builders.fr/forum/viewtopic.php?f=26&t=3928&hilit=BB8+par+MOUS
-  
+
   ===============
-  EXEMPLE COMMAND 
+  EXEMPLE COMMAND
   ===============
   < SET >
   'SET x87 y75 h3 \n'        // SET a new setpoint for x and y with 3degree of Hysteressis
   'SET x90 y90 i1 j0 k1 \n'  // SET an inversion of the sensor X Y and invert the direction of X
-  
+
   	x = SETPOINT for X
 	y = SETPOINT for Y
 	m = MULTIPLIER FOR THE SPEED OF THE MOTOR X and Y
 	i = INVERT the angle sensor ( X = Y and Y = X )
 	j = INVERT the direction of X
 	k = INVERT the direction of Y
-	h = SET a new HYSTERESSIS 
-
-  < BOOT > 
+	h = SET a new HYSTERESSIS
+
+  < BOOT >
   'BOOT \n'  // LAUNCH TEST for X, Y and Z -> Return X and Y to initial SETPOINT (x90 y90 ) and Z to ZERO
 
   < RUN >
   'RUN \n'   // RUN the code in production mode (Recieved all data, X and Y runing for the SETPOINT poistion and Z folow the remote controll
-  
-  < STOP >   
+
+  < STOP >
   'STOP \n'  // STOP the production mode. All motors are stopped.
-  
+
   < ROTATE >
   'ROTATE d1 s100 \n'   // ROTATE Z motor on the RIGHT DIRECTION (d0 LEFT, d1 RIGHT) WITH the SPEED at 1000/100 = 10 step/sec = 18dec/sec
-  
+
   < STAT >
   // It is all data SENDED to the remote
-  
-  < ENABLE > 
+
+  < ENABLE >
   'ENABLE a1 x1 z1 \n'    // ENABLE MOTOR AB, XY and Z (a = AB, x=XY, z = Z | 0 = desabled | 1 = enabled )
-  
-  
-  
-  FOR AFTER 
+
+
+
+  FOR AFTER
   http://tutorial.cytron.com.my/2014/05/15/wireless-uart-with-arduino-and-433mhz-or-434mhz-module/
   note : 10, 11, 12, 13, 14, 15, 50, 51, 52, 53
-  
+
 */
 
 #include "Wire.h"


#define Z_POSITION       16      // PIN FOR THE DIGITAL INPUT FOR THE HEAD POSITION

#define PING             13 

#define CurrentBat1      0
#define CurrentBat2      1

//=============================================================================//
void setup()
{
	// PINMODE
	pinMode( PIN_ENABLE_AB , OUTPUT );
	pinMode( PIN_ENABLE_XY , OUTPUT );
	pinMode( PIN_ENABLE_Z  , OUTPUT );
	pinMode( pinDirX       , OUTPUT );
	pinMode( pinStepX      , OUTPUT );
	pinMode( pinDirY       , OUTPUT );
	pinMode( pinStepY      , OUTPUT );
	pinMode( pinDirA       , OUTPUT );
	pinMode( PIN_STEP_A    , OUTPUT );
	pinMode( pinDirB       , OUTPUT );
	pinMode( pinStepB      , OUTPUT );
	pinMode( PING          , OUTPUT );
	pinMode( LIGHT         , OUTPUT );
	  
	pinMode( Z_POSITION    , INPUT  );

	// SERIAL
	Serial.begin(9600);

	// Blabla, blablabla
	Serial.println("==================================================");
	Serial.println("  _______ ______     _     _  ______  ____ ______ ");
	Serial.println(" (____   (____  )   | |   | |/ ___  |(____|______)");
	Serial.println("  ____)  )____)  )__| |   | | |   | | | |   | |   ");
	Serial.println(" |  __  (|  __  (___) |   | | |   | | | |   | |   ");
	Serial.println(" | |__)  ) |__)  )  | |___| | |   | |_| |_  | |   ");
	Serial.println(" |______/|______/   |______/|_|   |_(_____) |_|   ");
	Serial.println("            _ Astromech Industrie _               ");
	Serial.println("      > https://github.com/Gruniek/BB-UNIT <      ");
	Serial.println("           > http://r2builders.fr/ <              ");
	Serial.print("           FIRMWARE VERSION : ");
	Serial.print(version);
	Serial.println("               ");
	Serial.println("==================================================");

	Serial.println(" ");
	Serial.println("[1]-Booting up...");

	// Start of the I2C protocol
	Serial.print("[2]-Initializing I2C devices...  I2C ADRESS :");
	Serial.println(adress);
	Wire.begin(adress);

	// Start the MPU6050 Gyroscope
	Serial.print("[3]-Connect to the MPU6050 : ");
	accelgyro.initialize();
	//Serial.println(accelgyro.testConnection());
	Serial.println(accelgyro.testConnection() ? "Connection successful" : "Connection failed");

	// Start the RF24 Radio controll
	//Serial.println("[4]-Initializing RF24 devices...  ");
	//radio.begin();
	//radio.openReadingPipe(0, 00001);
	//radio.startListening();

	// Start remote connexion
	Serial.println("[4]-Initializing HC-12 devices [REMOTE] ");
	controler.begin(9600);

	// Start remote connexion
	if(ifHead)
	{
		Serial.println("[5]-Initializing HC-12 devices [ HEAD ] ");
		head.begin(9600);
	}
	// END OF BOOT/SETUP
	Serial.println("====================");
	Serial.println("Booting successful !");
	Serial.println("====================");
}

//=============================================================================//
