     delay(1000);
 }
 
+// Receive data from I2C communication
+void receiveData(int byteCount){
+    while (Wire.available()) {
+        byte dataReceived = Wire.read();
+        if (dataReceived == 0) { continue; }
+
+        if (command == 0) {
+            command = dataReceived;
+        }
+        else {
+            data = dataReceived;
+        }
+    }
+}
+
+void sendData(){
+    switch (command) {
+      case 1:
+          // Forward
+          movement_command = command;
+          movement_data = data;
+          is_movement_command_done = false;
+          break;
+
+      case 2:
+          // Backward
+          movement_command = command;
+          movement_data = data;
+          is_movement_command_done = false;
+          break;
+
+      case 3:
+          // Turn left
+          movement_command = command;
+          movement_data = data;
+          is_movement_command_done = false;
+          break;
+
+      case 4:
+          // Turn Rigth
+          movement_command = command;
+          movement_data = data;
+          is_movement_command_done = false;
+          break;
+
+      case 5:
+          // Stop
+          stop_motors();
+          break;
+
+      case 6:
+          // Set speed
+          speed_order = data;
+          break;
+
+      case 7:
+          // Distance
+          byte buf[] = { convert_imp_to_cm(counter_left),
+            convert_imp_to_cm(counter_right) };
+          Wire.write(buf, 2);
+          break;
+
+      case 8:
+          // Is Done
+          Wire.write(is_movement_command_done ? 1 : 0);
+          break;
+    }
+}
+
+void stop_motors() {
+    analogWrite(PWM_MOTOR_LEFT, 0);
+    analogWrite(PWM_MOTOR_RIGHT, 0);
+}
+
 // 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;
 }

// This code is made for an Arduino Mega and test if 
// the wheel encoder are working properly.

// Encoder wheel pins
const int IMP_ENCODER_LEFT_PIN = 2;

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);

