   uint32_t pre_time;
   uint8_t  toggle = 0;
 
-  CMD_SERIAL.println("Test Motor Left...");
-
-
-  if (tb3_id < 0)
-  {
-       CMD_SERIAL.println("    no dxl motors");
-  }
-  else
-  {
+  if (id == 1)
+  {
+    CMD_SERIAL.printf("Test Motor Left...");
+  }
+  else
+  {
+    CMD_SERIAL.printf("Test Motor Right...");
+  }
+  // We run at 1000000
+  portHandler->setBaudRate(1000000);
+
+  uint16_t model_number;
+  int dxl_comm_result = packetHandler2->ping(portHandler, id, &model_number);
+  if (dxl_comm_result == COMM_SUCCESS)
+  {
+    CMD_SERIAL.printf(" found type: %d\n", model_number);
     write(portHandler, packetHandler2, id, 64, 1, 1);
 
     toggle = 0;
     pre_time = millis();
     write(portHandler, packetHandler2, id, 104, 4, 100);
-    while(1)
+    while (1)
     {
       if (CMD_SERIAL.available())
       {

{
  uint8_t ch;

  while(CMD_SERIAL.available())
  {
    ch = CMD_SERIAL.read();
  }
}


bool findMotor(void)
{
  uint32_t baud_tbl[2] = { 57600, 1000000 };
  uint32_t index;
  uint32_t baud_pre;


char *dev_name = (char*)DEVICENAME;

// Initialize Packethandler2 instance
dynamixel::PacketHandler *packetHandler2;
dynamixel::PortHandler   *portHandler;


int tb3_id = -1;
