           }
         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
+            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_motor_speed_and_acceleration(motor_index, &motor_speed, &motor_acceleration);
-              sprintf(serial_out, "MP%d %d %d#", motor_index, (int)motor_speed, (int)motor_acceleration);
+              sprintf(serial_out, "GS%d %d %d#", motor_index, (int)motor_speed, (int)motor_acceleration);
               i += 4;
             }
             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, pot_dir;
-                byte pot_pin;
-                sscanf(tmp_str + i + 2, "%d", &pot_index);
-                potentiometers_controller.get_params(pot_index, &pot_pin, &pot_min, &pot_max, &pot_home, &pot_dir);
-                sprintf(serial_out, "PP%d %d %d %d %d %d#", pot_index, (int)pot_pin, pot_min, pot_max, pot_home, pot_dir);
-                i += 5;
-              }
-            else
-              if (tmp_str[i + 1] == 'U' || tmp_str[i + 1] == 'u'){ // get potentiometer min max home
-                int ultrasound_index;
-                byte trig_pin, echo_pin;
-                sscanf(tmp_str + i + 2, "%d", &ultrasound_index);
-                ultrasonic_sensors_controller.get_sensor_pins(ultrasound_index, &trig_pin, &echo_pin);
-                sprintf(serial_out, "UP%d %d %d#", ultrasound_index, trig_pin, echo_pin);
+              if (tmp_str[i + 1] == 'D' || tmp_str[i + 1] == 'd'){ // gets stepper motor speed and acceleration
+                int motor_index; 
+                byte motor_speed;
+                sscanf(tmp_str + i + 2, "%d", &motor_index);
+                dc_motors_controller_TB6612FNG.get_speed(motor_index, &motor_speed);
+                sprintf(serial_out, "GD%d %d#", motor_index, motor_speed);
                 i += 4;
               }
+              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, pot_dir;
+                  byte pot_pin;
+                  sscanf(tmp_str + i + 2, "%d", &pot_index);
+                  potentiometers_controller.get_params(pot_index, &pot_pin, &pot_min, &pot_max, &pot_home, &pot_dir);
+                  sprintf(serial_out, "GP%d %d %d %d %d %d#", pot_index, (int)pot_pin, pot_min, pot_max, pot_home, pot_dir);
+                  i += 5;
+                }
+                else
+                  if (tmp_str[i + 1] == 'U' || tmp_str[i + 1] == 'u'){ // get potentiometer min max home
+                    int ultrasound_index;
+                    byte trig_pin, echo_pin;
+                    sscanf(tmp_str + i + 2, "%d", &ultrasound_index);
+                    ultrasonic_sensors_controller.get_sensor_pins(ultrasound_index, &trig_pin, &echo_pin);
+                    sprintf(serial_out, "GU%d %d %d#", ultrasound_index, trig_pin, echo_pin);
+                    i += 4;
+                  }
         }
         else
          if (tmp_str[i] == 'T' || tmp_str[i] == 't'){// test connection

                   num_consumed_total += num_consumed + 1;
                 }
     
                 sprintf(serial_out, "CM#");
                 i += num_consumed_total;
               }
             }

#include "button_controller.h"
#include "jenny5_types.h"
#include "infrared_sensors_controller.h"
#include "dc_motors_controller_TB6612FNG.h"

//#include "MemoryFree.h"

t_stepper_motors_controller stepper_motors_controller;
t_dc_motors_controller_TB6612FNG dc_motors_controller_TB6612FNG;
t_potentiometers_controller potentiometers_controller;
t_ultrasonic_sensors_controller ultrasonic_sensors_controller;
t_infrared_sensors_controller infrared_sensors_controller;
