 
   CMD_SERIAL.println("Find Motor...");
 
-  for (index=0; index<2; index++)
+  // First try to find the specific servo ID wanted
+  for (index = 0; index < COUNT_BAUD; index++)
+  {
+    portHandler->setBaudRate(baud_tbl[index]);
+    uint16_t model_number;
+    int dxl_comm_result = packetHandler2->ping(portHandler, id, &model_number);
+    if (dxl_comm_result == COMM_SUCCESS)
+    {
+      if (tb3_id == -1)
+      {
+        tb3_id = id;
+        tb3_baud =  baud_tbl[index];
+      }
+      else
+      {
+        CMD_SERIAL.printf("Warning Servo %d found at two baud rates %d and %d using %d\n",
+                          id, tb3_baud, baud_tbl[index], baud_tbl[index]);
+        tb3_baud =  baud_tbl[index];
+      }
+    }
+  }
+
+  if (tb3_id != -1)
+  {
+    CMD_SERIAL.println("    ... SUCCESS");
+    CMD_SERIAL.printf("    [ID: %d found at baud: %d]\n", id, tb3_baud );
+    return true;
+  }
+
+  // Did not find the actual ID we were looking for so see if we find any servos?
+  for (index = 0; index < COUNT_BAUD; index++)
   {
     CMD_SERIAL.printf("    setbaud : %d\r\n", baud_tbl[index]);
 
     portHandler->setBaudRate(baud_tbl[index]);
     tb3_baud =  baud_tbl[index];
 
+    // Lets see if we find the actual one we want to update?
     int dxl_comm_result = packetHandler2->broadcastPing(portHandler, vec);
     if (dxl_comm_result != COMM_SUCCESS)
     {


  CMD_SERIAL.print("Do you really want to setup ? y/n : ");

  while(1)
  {
    if (CMD_SERIAL.available())
    {


  if (tb3_id < 0)
  {
    CMD_SERIAL.println("    no dxl motors");
  }
  else
  {
    write(portHandler, packetHandler2, tb3_id, 64, 1, 0);
    write(portHandler, packetHandler2, tb3_id, 7, 1, 1);
    tb3_id = 1;
    write(portHandler, packetHandler2, tb3_id, 8, 1, 3);
