               stepper_motors_controller.add_sensor(motor_index, BUTTON, sensor_index);
             else if (motor_type == 'D' || motor_type == 'd') // attach to DC motors controller
               dc_motors_controller_TB6612FNG.add_sensor(motor_index, BUTTON, sensor_index);
-            i += 2;
+            j += 2;
+            k++;
           }
           else if (tmp_str[j] == 'I' || tmp_str[j] == 'i') {
             int sensor_index;
             sscanf(tmp_str + j + 1, "%d", &sensor_index);
+            Serial.print(motor_index);
+            Serial.write(' ');
+            Serial.println(sensor_index);
             if (motor_type == 'S' || motor_type == 's') // attach to stepper motors controller
               stepper_motors_controller.add_sensor(motor_index, INFRARED, sensor_index);
             else if (motor_type == 'D' || motor_type == 'd') // attach to DC motors controller
               dc_motors_controller_TB6612FNG.add_sensor(motor_index, INFRARED, sensor_index);
-            i += 2;
+            j += 2;
+            k++;
           }
           else
-            i++;
-          j++;
-        }
-        i++;
+            j++;
+        }
+        i = j + 1;
+       // Serial.println(i);
         continue;
       }
 
       if (tmp_str[i] == 'G' || tmp_str[i] == 'g') { // for debugging purpose
         if (tmp_str[i + 1] == 'S' || tmp_str[i + 1] == 's') { // gets stepper motor speed and acceleration
           int motor_index;
+          byte num_sensors;
           float motor_speed, motor_acceleration;
           sscanf(tmp_str + i + 2, "%d", &motor_index);
           stepper_motors_controller.get_speed_and_acceleration(motor_index, &motor_speed, &motor_acceleration);
-          sprintf(tmp_serial_out, "GS%d %d %d#", motor_index, (int)motor_speed, (int)motor_acceleration);
+          num_sensors = stepper_motors_controller.get_num_attached_sensors(motor_index);
+          sprintf(tmp_serial_out, "GS%d %d %d %d#", motor_index, (int)motor_speed, (int)motor_acceleration, num_sensors);
           strcat(serial_out, tmp_serial_out);
           i += 4;
         }

        // now I have to add sensors one by one
        int j = i + 4;
        i += 5;
        while (j < str_length) {
          if (tmp_str[j] == 'P' || tmp_str[j] == 'p') {
            int sensor_index;
            sscanf(tmp_str + j + 1, "%d", &sensor_index);

void setup()
{
  first_start = 0;
  strcpy(firmware_version, "2016.05.12.0");

  current_buffer[0] = 0;

