text: /*******************************************************************************
  * Copyright 2017 ROBOTIS CO., LTD.
  *
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  *
  *     http://www.apache.org/licenses/LICENSE-2.0
  *
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  *******************************************************************************/
  
  /* Author: Ryu Woon Jung (Leon) */
  
  //
  // *********     Factory Reset Example      *********
  //
  //
  // Available Dynamixel model on this example : All models using Protocol 1.0
  // This example is tested with a Dynamixel MX-28, and an USB2DYNAMIXEL
  // Be sure that Dynamixel PRO properties are already set as %% ID : 1 / Baudnum : 34 (Baudrate : 57600)
  //
  
  // Be aware that:
  // This example resets all properties of Dynamixel to default values, such as %% ID : 1 / Baudnum : 34 (Baudrate : 57600)
  //
  
  #if defined(__linux__) || defined(__APPLE__)
  #include <fcntl.h>
  #include <termios.h>
  #include <unistd.h>
  #define STDIN_FILENO 0
  #elif defined(_WIN32) || defined(_WIN64)
  #include <conio.h>
  #endif
  
  #include <stdio.h>
  
  #include "dynamixel_sdk.h"                                  // Uses Dynamixel SDK library
  
  // Control table address
  #define ADDR_MX_BAUDRATE                4                   // Control table address is different in Dynamixel model
  
  // Protocol version
  #define PROTOCOL_VERSION                1.0                 // See which protocol version is used in the Dynamixel
  
  // Default setting
  #define DXL_ID                          1                   // Dynamixel ID: 1
  #define BAUDRATE                        57600
  #define DEVICENAME                      "/dev/ttyUSB0"      // Check which port is being used on your controller
                                                              // ex) Windows: "COM1"   Linux: "/dev/ttyUSB0" Mac: "/dev/tty.usbserial-*"
  
  #define FACTORYRST_DEFAULTBAUDRATE      57600               // Dynamixel baudrate set by factoryreset
  #define NEW_BAUDNUM                     1                   // New baudnum to recover Dynamixel baudrate as it was
  #define OPERATION_MODE                  0x00                // Mode is unavailable in Protocol 1.0 Reset
  
  #define TORQUE_ENABLE                   1                   // Value for enabling the torque
  #define TORQUE_DISABLE                  0                   // Value for disabling the torque
  
  int getch()
  {
  #if defined(__linux__) || defined(__APPLE__)
    struct termios oldt, newt;
    int ch;
    tcgetattr(STDIN_FILENO, &oldt);
    newt = oldt;
    newt.c_lflag &= ~(ICANON | ECHO);
    tcsetattr(STDIN_FILENO, TCSANOW, &newt);
    ch = getchar();
    tcsetattr(STDIN_FILENO, TCSANOW, &oldt);
    return ch;
  #elif defined(_WIN32) || defined(_WIN64)
    return _getch();
  #endif
  }
  
  int kbhit(void)
  {
  #if defined(__linux__) || defined(__APPLE__)
    struct termios oldt, newt;
    int ch;
    int oldf;
  
    tcgetattr(STDIN_FILENO, &oldt);
    newt = oldt;
    newt.c_lflag &= ~(ICANON | ECHO);
    tcsetattr(STDIN_FILENO, TCSANOW, &newt);
    oldf = fcntl(STDIN_FILENO, F_GETFL, 0);
    fcntl(STDIN_FILENO, F_SETFL, oldf | O_NONBLOCK);
  
    ch = getchar();
  
    tcsetattr(STDIN_FILENO, TCSANOW, &oldt);
    fcntl(STDIN_FILENO, F_SETFL, oldf);
  
    if (ch != EOF)
    {
      ungetc(ch, stdin);
      return 1;
    }
  
    return 0;
  #elif defined(_WIN32) || defined(_WIN64)
    return _kbhit();
  #endif
  }
  
  void msecSleep(int waitTime)
  {
  #if defined(__linux__) || defined(__APPLE__)
    usleep(waitTime * 1000);
  #elif defined(_WIN32) || defined(_WIN64)
    _sleep(waitTime);
  #endif
  }
  
  int main()
  {
    // Initialize PortHandler instance
    // Set the port path
    // Get methods and members of PortHandlerLinux or PortHandlerWindows
    dynamixel::PortHandler *portHandler = dynamixel::PortHandler::getPortHandler(DEVICENAME);
  
    // Initialize PacketHandler instance
    // Set the protocol version
    // Get methods and members of Protocol1PacketHandler or Protocol2PacketHandler
    dynamixel::PacketHandler *packetHandler = dynamixel::PacketHandler::getPacketHandler(PROTOCOL_VERSION);
  
    int dxl_comm_result = COMM_TX_FAIL;             // Communication result
  
    uint8_t dxl_error = 0;                          // Dynamixel error
    uint8_t dxl_baudnum_read;                       // Read baudnum
  
    // Open port
    if (portHandler->openPort())
    {
      printf("Succeeded to open the port!\n");
    }
    else
    {
      printf("Failed to open the port!\n");
      printf("Press any key to terminate...\n");
      getch();
      return 0;
    }
  
    // Set port baudrate
    if (portHandler->setBaudRate(BAUDRATE))
    {
      printf("Succeeded to change the baudrate!\n");
    }
    else
    {
      printf("Failed to change the baudrate!\n");
      printf("Press any key to terminate...\n");
      getch();
      return 0;
    }
  
    // Read present baudrate of the controller
    printf("Now the controller baudrate is : %d\n", portHandler->getBaudRate());
  
    // Try factoryreset
    printf("[ID:%03d] Try factoryreset : ", DXL_ID);
    dxl_comm_result = packetHandler->factoryReset(portHandler, DXL_ID, OPERATION_MODE, &dxl_error);
    if (dxl_comm_result != COMM_SUCCESS)
    {
      printf("Aborted\n");
      printf("%s\n", packetHandler->getTxRxResult(dxl_comm_result));
      return 0;
    }
    else if (dxl_error != 0)
    {
      printf("%s\n", packetHandler->getRxPacketError(dxl_error));
    }
  
    // Wait for reset
    printf("Wait for reset...\n");
    msecSleep(2000);
  
    printf("[ID:%03d] factoryReset Success!\n", DXL_ID);
  
    // Set controller baudrate to dxl default baudrate
    if (portHandler->setBaudRate(FACTORYRST_DEFAULTBAUDRATE))
    {
      printf("Succeed to change the controller baudrate to : %d\n", FACTORYRST_DEFAULTBAUDRATE);
    }
    else
    {
      printf("Failed to change the controller baudrate\n");
      printf("Press any key to terminate...\n");
      getch();
      return 0;
    }
  
    // Read Dynamixel baudnum
    dxl_comm_result = packetHandler->read1ByteTxRx(portHandler, DXL_ID, ADDR_MX_BAUDRATE, &dxl_baudnum_read, &dxl_error);
    if (dxl_comm_result != COMM_SUCCESS)
    {
      printf("%s\n", packetHandler->getTxRxResult(dxl_comm_result));
    }
    else if (dxl_error != 0)
    {
      printf("%s\n", packetHandler->getRxPacketError(dxl_error));
    }
    else
    {
      printf("[ID:%03d] Dynamixel baudnum is now : %d\n", DXL_ID, dxl_baudnum_read);
    }
  
    // Write new baudnum
    dxl_comm_result = packetHandler->write1ByteTxRx(portHandler, DXL_ID, ADDR_MX_BAUDRATE, NEW_BAUDNUM, &dxl_error);
    if (dxl_comm_result != COMM_SUCCESS)
    {
      printf("%s\n", packetHandler->getTxRxResult(dxl_comm_result));
    }
    else if (dxl_error != 0)
    {
      printf("%s\n", packetHandler->getRxPacketError(dxl_error));
    }
    else
    {
      printf("[ID:%03d] Set Dynamixel baudnum to : %d\n", DXL_ID, NEW_BAUDNUM);
    }
  
    // Set port baudrate to BAUDRATE
    if (portHandler->setBaudRate(BAUDRATE))
    {
      printf("Succeed to change the controller baudrate to : %d\n", BAUDRATE);
    }
    else
    {
      printf("Failed to change the controller baudrate\n");
      printf("Press any key to terminate...\n");
      getch();
      return 0;
    }
  
    msecSleep(200);
  
    // Read Dynamixel baudnum
    dxl_comm_result = packetHandler->read1ByteTxRx(portHandler, DXL_ID, ADDR_MX_BAUDRATE, &dxl_baudnum_read, &dxl_error);
    if (dxl_comm_result != COMM_SUCCESS)
    {
      printf("%s\n", packetHandler->getTxRxResult(dxl_comm_result));
    }
    else if (dxl_error != 0)
    {
      printf("%s\n", packetHandler->getRxPacketError(dxl_error));
    }
    else
    {
      printf("[ID:%03d] Dynamixel Baudnum is now : %d\n", DXL_ID, dxl_baudnum_read);
    }
  
    // Close port
    portHandler->closePort();
  
    return 0;
  }
  