  // VAR INITIALIZATION
     encoder0_cnt = 0;
     encoder1_cnt = 0;
-    data_raw.data_length = 13; // The number of output message entries
+    data_raw.data_length = DATA_LENGTH; // The number of output message entries
     data_raw.data = (float *)malloc(sizeof(float)*data_raw.data_length);
  //MOTORS           
     md.init();
  //ROS        
     nh.initNode();
-    nh.subscribe(sub);
+    nh.subscribe(sub_motors);
+    nh.subscribe(sub_srv_clapper);
+    nh.subscribe(sub_srv_carpet);
     nh.advertise(pub_array);
 }
 
 void loop()
 {  
-    if ( (millis()-publish_timer) > 200) {
+    if ( (millis()-publish_timer) > 20) {
         MagnetometerScaled scaled = compass.ReadScaledAxis();      
  //IMU
         data_raw.data[IMU_ANG_VX] = ToD((float)(imu.GyroX()));


//ROS-TOPICS
ros::Publisher pub_array("apm/data_raw",&data_raw);
ros::Subscriber<geometry_msgs::Twist> sub("cmd_vel", motor_cb);

void setup() {
 //I2C        

        data_raw.data[ENC_DATA_1] = encoder1_cnt;
        encoder0_cnt = 0;
        encoder1_cnt = 0;
 //BUTTON
        data_raw.data[BTN_STATE] = digitalRead(BTN_PIN);
 //ROS                     
        pub_array.publish(&data_raw);
        publish_timer = millis();
