 }
 
 void send_data_to_ROS() {
-  Serial.println(msg_from_robot[0]); //Serial.write() may be a better choice for this
-  Serial.println(msg_from_robot[1]); //Serial.write() may be a better choice for this
+  //Serial.println(msg_from_robot[0]); //Serial.write() may be a better choice for this
+  //Serial.println(msg_from_robot[1]); //Serial.write() may be a better choice for this
 }
 
 bool receive_data_from_ROS() {
   if (Serial.available()) {
     int i = 0;
-    while (Serial.available()) {
+    while (i<3) {
       msg_from_ROS[i++] = Serial.read() - 48;
+      Serial.println(msg_from_ROS[i-1]);
     }
     parseROStoRobot();
     return true;


  delay(10);
}


  radio.setChannel(108);

  radio.openReadingPipe(1, pipe[1]); //n1->central pelo pipe0
  radio.openReadingPipe(2, pipe[3]); //n1->central pelo pipe0
  radio.openReadingPipe(3, pipe[5]); //n1->central pelo pipe0
  radio.openReadingPipe(4, pipe[7]); //n1->central pelo pipe0
  radio.startListening();
}

