             int sensor_index;
             sscanf(tmp_str + i + 1, "%d", &sensor_index);
             Serial.write('U');
+            Serial.print(sensor_index);
+            Serial.write(' ');
             Serial.print(ultrasonic_sensors_controller.getDistanceForSensor(sensor_index));
             Serial.write('#');
             i++;

            int sensor_index;
            sscanf(tmp_str + i + 1, "%d", &sensor_index);
            Serial.write('I');
            Serial.print(infrared_sensors_control.get_distance(sensor_index));
            Serial.write('#');
            i++;
          }
        else
          if (tmp_str[i] == 'G' || tmp_str[i] == 'g'){// for debugging purpose
                        if (tmp_str[i + 1] == 'M' || tmp_str[i + 1] == 'm'){ // get motor speed and acceleration
              int motor_index, motor_speed, motor_acceleration;
              sscanf(tmp_str + i + 2, "%d", &motor_index);
              motors_control.get_motor_speed_and_acceleration(motor_index, &motor_speed, &motor_acceleration);

              Serial.print(motor_speed); 
              Serial.write(' ');
              Serial.print(motor_acceleration); 
            }
            else
              if (tmp_str[i + 1] == 'P' || tmp_str[i + 1] == 'p'){ // get potentiometer min max home
                int pot_index, pot_min, pot_max, pot_home;
                sscanf(tmp_str + i + 2, "%d", &pot_index);
                potentiometers_control.get_limits(pot_index, &pot_min, &pot_max, &pot_home);
                Serial.write("PP"); // potentiometer parameters
                Serial.print(pot_index); 
                Serial.write(' ');
                Serial.print(pot_min);
                Serial.write(' ');
                Serial.print(pot_max);
                Serial.write(' ');
                Serial.print(pot_home);
                Serial.write('#'); 
              }
              else{
                          // for all motors, unformated data
            int index;
            for(index = 0; index < motors_control.num_motors; index++) {
