Optimize motors ping()

Author: facchinm

src/Braccio.cpp

@@ -213,14 +213,16 @@ void BraccioClass::motors_connected_thread() {
 	      _connected[i] = (servos->ping(i) == 0);
 	      //Serial.print(String(i) + ": ");
 	      //Serial.println(_connected[i]);
-	      i2c_mutex.lock();
+	    }
+	    i2c_mutex.lock();
+	    for (int i = 1; i < 7; i++) {
 	      if (_connected[i]) {
 	        setGreen(i);
 	      } else {
 	        setRed(i);
 	      }
-	      i2c_mutex.unlock();
 	    }
+	    i2c_mutex.unlock();
 	  }
     delay(1000);
   }

src/lib/motors/SmartServo.cpp.impl

@@ -63,7 +63,9 @@ template <int MAX_MOTORS> void SmartServoClass<MAX_MOTORS>::receiveResponse() {
   	//Serial.print(c, HEX);
   	//Serial.print(" ");
     _rxBuf[_rxLen++]=c;
-    delayMicroseconds(2000);
+    if (!_RS485.available()) {
+      delay(1);
+    }
   }
   //Serial.println();
   _RS485.noReceive();