Merge pull request #53 from arduino-libraries/fix-run-through-all-servos

Fix: Do not require all servos to be connected when running `Factory_Set_Initial_Servo_Position`

Author: aentinger

examples/Tools/Factory_Set_Initial_Servo_Position/Factory_Set_Initial_Servo_Position.ino

@@ -49,9 +49,22 @@ static std::array<InitialServoPosition, 6> const INITIAL_SERVO_POSITION =
 
 bool set_initial_servo_position(int const id, float const target_angle)
 {
+  auto isTimeout = [](unsigned long const start) -> bool { return ((millis() - start) > 2000); };
+
+  auto start = millis();
+
   Serial.print("Connecting .... ");
-  for (; !Braccio.get(id).connected(); delay(10)) { }
-  Serial.println("OK.");
+  for (; !Braccio.get(id).connected() && !isTimeout(start); delay(10)) { }
+  if (!isTimeout(start))
+    Serial.println("OK.");
+  else
+  {
+    Serial.print("Error: Can not connect to servo ");
+    Serial.print(id);
+    Serial.println(" within time limit.");
+    Serial.println();
+    return false;
+  }
   delay(500);
 
   Serial.print("Disengaging ... ");
@@ -65,14 +78,12 @@ bool set_initial_servo_position(int const id, float const target_angle)
   delay(500);
 
   /* Drive to the position for assembling the servo horn. */
-  auto const start = millis();
-  auto isTimeout = [start]() -> bool { return ((millis() - start) > 5000); };
   auto isTargetAngleReached = [target_angle, id](float const epsilon) -> bool { return (fabs(Braccio.get(id).position() - target_angle) <= epsilon); };
 
   static float constexpr EPSILON = 2.0f;
 
   for ( float current_angle = Braccio.get(id).position();
-       !isTargetAngleReached(EPSILON) && !isTimeout();)
+       !isTargetAngleReached(EPSILON) && !isTimeout(start);)
   {
     Braccio.get(id).move().to(target_angle).in(200ms);
     delay(250);
@@ -82,13 +93,21 @@ bool set_initial_servo_position(int const id, float const target_angle)
     Serial.println(msg);
   }
 
-  if (!isTargetAngleReached(EPSILON)) {
-    Serial.println("Servo did not reach target angle.");
+  if (!isTargetAngleReached(EPSILON))
+  {
+    Serial.print("Error: Servo ");
+    Serial.print(id);
+    Serial.print(" did not reach target angle.");
+    Serial.println();
     return false;
   }
 
-  if (isTimeout()) {
-    Serial.println("Timeout error.");
+  if (isTimeout(start))
+  {
+    Serial.print("Error: Servo ");
+    Serial.print(id);
+    Serial.println(" did not reach target angle within time limit.");
+    Serial.println();
     return false;
   }
 
@@ -104,29 +123,39 @@ void setup()
   Serial.begin(115200);
   while(!Serial) { }
 
-  if (!Braccio.begin()) {
+  if (!Braccio.begin(nullptr, false)) {
     Serial.println("Braccio.begin() failed.");
     for(;;) { }
   }
 
   Braccio.disengage();
 
+  int success_cnt = 0;
   for (auto & servo : INITIAL_SERVO_POSITION)
   {
     Serial.print("Servo ");
     Serial.print(servo.id());
     Serial.print(": Target Angle = ");
     Serial.print(servo.angle());
-    Serial.print(" °");
     Serial.println();
 
-    if (!set_initial_servo_position(servo.id(), servo.angle())) {
-      Serial.println("ERROR.");
-      return;
-    }
+    if (set_initial_servo_position(servo.id(), servo.angle()))
+      success_cnt++;
   }
 
-  Serial.println("SUCCESS : all servos are set to their initial position.");
+  if (success_cnt == SmartServoClass::NUM_MOTORS)
+  {
+    Serial.println("SUCCESS : all servos are set to their initial position.");
+  }
+  else
+  {
+    Serial.print("ERROR: only ");
+    Serial.print(success_cnt);
+    Serial.print(" of ");
+    Serial.print(SmartServoClass::NUM_MOTORS);
+    Serial.print(" could be set to the desired initial position.");
+    Serial.println();
+  }
 }
 
 void loop()

src/Braccio++.cpp

@@ -93,7 +93,7 @@ BraccioClass::BraccioClass()
  * PUBLIC MEMBER FUNCTIONS
  **************************************************************************************/
 
-bool BraccioClass::begin(voidFuncPtr custom_menu)
+bool BraccioClass::begin(voidFuncPtr custom_menu, bool const wait_for_all_motor_connected)
 {
   static int constexpr RS485_RX_PIN = 1;
 
@@ -140,13 +140,16 @@ bool BraccioClass::begin(voidFuncPtr custom_menu)
 
   _motors_connected_thd.start(mbed::callback(this, &BraccioClass::motorConnectedThreadFunc));
 
-  /* Wait for all motors to be actually connected. */
-  for (int id = SmartServoClass::MIN_MOTOR_ID; id <= SmartServoClass::MAX_MOTOR_ID; id++)
+  if (wait_for_all_motor_connected)
   {
-    auto const start = millis();
-    auto isTimeout = [start]() -> bool { return ((millis() - start) > 5000); };
-    for(; !isTimeout() && !connected(id); delay(100)) { }
-    if (isTimeout()) return false;
+    /* Wait for all motors to be actually connected. */
+    for (int id = SmartServoClass::MIN_MOTOR_ID; id <= SmartServoClass::MAX_MOTOR_ID; id++)
+    {
+      auto const start = millis();
+      auto isTimeout = [start]() -> bool { return ((millis() - start) > 5000); };
+      for(; !isTimeout() && !connected(id); delay(100)) { }
+      if (isTimeout()) return false;
+    }
   }
 
   return true;

src/Braccio++.h

@@ -65,8 +65,7 @@ class BraccioClass
   BraccioClass();
 
   inline bool begin() { return begin(nullptr); }
-         bool begin(voidFuncPtr custom_menu);
-
+         bool begin(voidFuncPtr custom_menu, bool const wait_for_all_motor_connected = true);
 
   void pingOn();
   void pingOff();