 int  SETPOINT_X         = 90;
 int  SETPOINT_Y         = 90;
 
-int  Z_ANGLE            = 0;
+//int  Z_ANGLE            = 0;
 bool NEW_DATA           = false;
 
 int  MULTIPLIER         = 15;

//=============================================================================//
void setup()
{
	// PINMODE
	pinMode( PIN_ENABLE_A    , OUTPUT );
	pinMode( PIN_ENABLE_B    , OUTPUT );
	pinMode( PIN_DIRECTION_X , OUTPUT );
	pinMode( PIN_STEP_X      , OUTPUT );
	pinMode( PIN_DIRECTION_Y , OUTPUT );
	pinMode( PIN_STEP_Y      , OUTPUT );
	pinMode( PIN_DIR_A       , OUTPUT );
	pinMode( PIN_STEP_A      , OUTPUT );
	pinMode( PIN_DIR_B       , OUTPUT );
	pinMode( PIN_STEP_B      , OUTPUT );
	pinMode( LIGHTING        , OUTPUT );
	pinMode( LED_1           , OUTPUT );
	pinMode( LED_2           , OUTPUT );
	pinMode( LED_3           , OUTPUT );
	pinMode( LED_4           , OUTPUT );
	pinMode( LED_5           , OUTPUT );
	  
	pinMode( Z_POSITION      , INPUT  );
	pinMode( TRIGGER_LIGHT   , INPUT  );
	pinMode( SWITCH_1        , INPUT  );
	
	// SERIAL
	Serial.begin(9600);

	// Blabla, blablabla
	Serial.println("==================================================");
	Serial.println("  _______ ______     _     _  ______  ____ ______ ");
	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.println("                                                  ");
	Serial.println("           > Code by Daniel Mostosi <             ");
	Serial.println("                                                  ");
	Serial.println("                Report an issues?                 ");
	Serial.println("    https://github.com/Gruniek/BB-UNIT/issues     ");
	Serial.println("                                                  ");
	Serial.print("           FIRMWARE VERSION : ");
	Serial.print(FIRMWARE_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(I2C_ADRESS);
	Wire.begin(I2C_ADRESS);

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

	Serial.print("[4]-Connect to the MPU6050 BODY: ");
    accelgyroIC2.initialize();
   	Serial.println(accelgyroIC2.testConnection() ? "Connection successful" : "Connection failed");

	//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("[5]-Initializing HC-12 devices [REMOTE] ");
	controler.begin(9600);

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

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


//PINOUT

//#define PIN_ENABLE_A     33      // Activation off all stepper
//#define PIN_ENABLE_B     27      // Activation off all stepper

#define PIN_STEP_A       4      // PIN STEP FOR X
#define PIN_DIR_A        5      // PIN DIRECTION FOR X 
#define PIN_STEP_B       6      // PIN STEP FOR X
#define PIN_DIR_B        7      // PIN DIRECTION FOR X 
#define PIN_ENABLE_AB    9 

#define PIN_ENABLE_X     43
#define PIN_STEP_X       51      // PIN STEP FOR X        //26
#define PIN_DIRECTION_X  53      // PIN DIRECTION FOR X   //24 
#define PIN_X_MS_1       45
#define PIN_X_MS_2       47
#define PIN_X_MS_3       49

#define PIN_ENABLE_Y     31     
#define PIN_STEP_Y       39       // PIN STEP FOR Y       // 4
#define PIN_DIRECTION_Y  41       // PIN DIRECTION FOR Y  // 3
#define PIN_Y_MS_1       33
#define PIN_Y_MS_2       35
#define PIN_Y_MS_3       37

#define PIN_ENABLE_Z     23
#define PIN_STEP_Z       27      // PIN STEP FOR Y
#define PIN_DIRECTION_Z  29       // PIN DIRECTION FOR Y
#define PIN_Z_MS_1       25
#define Z_POSITION       22

#define BP_LEFT          52
#define BP_RIGH          50

#define LED_1            48
#define LED_2            46
#define LED_3            44
#define LED_4            42
#define LED_5            40
#define LED_6            38

#define TRIGGER_LIGHT    24      // PIN TRIGGER FOR START/STOP THE LIGHTING INSIDE THE BALL
#define LIGHTING         26      // PIN FOR FEED THE LIGHTING

#define POWER_1          28
#define POWER_2          30
#define AUX_1			 32
#define AUX_2            34
#define AUX_3            36

#define BUZZER           3

#define PING             13
#define PIN_RESET        12


//#define LIGHT            20      // Pin for turn on/off lighting on the ball (Great for a technical maintenance ;) )

//#define CURRENT          0
//#define VOLTAGE          6

// I2C
int  I2C_ADRESS         = 1;
