               stepper_motors_controller.add_sensor(motor_index, POTENTIOMETER, sensor_index);
             else if (motor_type == 'D' || motor_type == 'd') // attach to DC motors controller
               dc_motors_controller_TB6612FNG.add_sensor(motor_index, POTENTIOMETER, sensor_index);
-            i += 2;
+            j += 2;
+            k++;
           }
           else if (tmp_str[j] == 'B' || tmp_str[j] == 'b') {
             int sensor_index;

              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;
          }
          else if (tmp_str[j] == 'I' || tmp_str[j] == 'i') {
            int sensor_index;
            sscanf(tmp_str + j + 1, "%d", &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;
          }
          else
            i++;
          j++;
        }
        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;
          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);
          strcat(serial_out, tmp_serial_out);
          i += 4;
        }

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