-// This code is made for an Arduino Mega and test if 
+#include <Wire.h>
+
+// This code is made for an Arduino Mega and test if
 // the wheel encoder are working properly.
+
+// I2C address
+const byte SLAVE_ADDRESS = 0x05;
+
+// I2C variables
+byte command = 0;
+byte data = 0;
+
+
+byte movement_command = 0;
+byte movement_data = 0;
+byte speed_order = 0;
+bool is_movement_command_done = true;
 
 // Encoder wheel pins
 const int IMP_ENCODER_LEFT_PIN = 2;

    delay(1000);
}

// Interrupt callbacks

void encoder_pulse_left() {
    //Serial.println("encoder left pulse");
    
    int direction = digitalRead(DIRECTION_LEFT_PIN);
    counter_left += (direction == 1) ? 1 : -1;
}

void encoder_pulse_right() {
    //Serial.println("encoder right pulse");
    
    int direction = digitalRead(DIRECTION_RIGHT_PIN);
    counter_right += (direction == 0) ? 1 : -1;
}

long counter_left = 0;
long counter_right = 0;


void setup() {
    Serial.begin(9600);

    Wire.begin(SLAVE_ADDRESS);
    Wire.onReceive(receiveData);
    Wire.onRequest(sendData);

    // Generate interrupt number from pins.
    int int_left = digitalPinToInterrupt(IMP_ENCODER_LEFT_PIN);
    int int_right = digitalPinToInterrupt(IMP_ENCODER_RIGHT_PIN);

    attachInterrupt(int_left, encoder_pulse_left, CHANGE);
    attachInterrupt(int_right, encoder_pulse_right, CHANGE);

