 #include "jenny5_types.h"
 #include "infrared_sensors_controller.h"
 
-byte potentiometers_pins[4] = {0, 1, 2, 3};
-t_limit_pair potentiometer_limits[4] = {{500, 1000, 500}, {500, 1023, 500}, {200, 600, 400}, {300, 600, 500}};
-
-byte ultrasonic_trig_pins[2] = {8, 50};
-byte ultrasonic_echo_pins[2] = {9, 51};
+//byte potentiometers_pins[4] = {0, 1, 2, 3};
+//t_limit_pair potentiometer_limits[4] = {{500, 1000, 500}, {500, 1023, 500}, {200, 600, 400}, {300, 600, 500}};
 
 byte infrared_pins[2] = {52, 53};
 
-t_motors_control motors_control(4);
-t_potentiometers_controller potentiometers_control (4, potentiometers_pins, potentiometer_limits);
-t_ultrasonic_sensors_controller ultrasonic_sensors_controller (1, ultrasonic_trig_pins, ultrasonic_echo_pins);
-t_infrared_sensors_controller infrared_sensors_control(2, infrared_pins);
+t_motors_controller motors_controller;
+t_potentiometers_controller potentiometers_control;
+t_ultrasonic_sensors_controller ultrasonic_sensors_controller;
+t_infrared_sensors_controller *infrared_sensors_control;
 
 char is_command_running;
 
-char firmware_version[] = "2016.01.19.2";// year.month.day.build number
+char firmware_version[20];// year.month.day.build number
 
 char current_buffer[65];
 

  Serial.println(F("SMx s a# // Sets speed of motor x to s and the acceleration to a."));
  Serial.println(F("SPx min max home# // Sets the parameters of a potentiometer. Min and max are the limits where it can move and home is from where we bring the robot when we start."));
  Serial.println(F("Ax n Py Bz ... # // Attach to motor x a list of n sensors (like Potentiometer y, Button z etc)."));
  Serial.println(F("Ux# // Gets the distance as measured by the ultrasonic sensor x. Outputs Ux d#."));
  Serial.println(F("Bx# // Gets the status of the button x. Outputs Bx s#"));
  Serial.println(F("Px# // Gets the position of the potentiometer x. Outputs Px p#"));
  Serial.println(F("Ix# // Gets the value of infrared sensor x. Outputs Ix v#"));
  Serial.println(F("GMx# // Gets the parameters for motor x: speed acceleration num_sensors sensor_index1, sensor_type1 sensor_index1, sensor_type1. Outputs MPx s a 1 0 0#"));
  Serial.println(F("GPx# // Gets the parameters for potentiometer x: min max home. Outputs Px l u h#"));
  Serial.println(F("V# // Outputs version string. eg: 2016.01.17.0#"));

  

            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_controller.get_motor_speed_and_acceleration(motor_index, &motor_speed, &motor_acceleration);
              sprintf(serial_out, "MP%d %d %d#", motor_index, motor_speed, 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;
                byte pot_pin;
                sscanf(tmp_str + i + 2, "%d", &pot_index);
                potentiometers_control.get_params(pot_index, &pot_pin, &pot_min, &pot_max, &pot_home);
                sprintf(serial_out, "PP%d %d %d %d %d#", pot_index, (int)pot_pin, pot_min, pot_max, pot_home);
                i += 4;
              }
            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);
                i += 4;
              }
        }
