Dynamixel SDK - Find Servos Test program

I am not sure if anyone will get any usage out of this test program. But I thought I might post it as there have been a few different postings/questions up on the Robotis forum about people having difficulties talking to their servos connected up to an OpenCM9.04 board.

So I extracted some of my larger servo test program for the OpenCM9.04 board into into a simpler test program that tries to print out which servos are connected to the OpenCM board on either Serial1 or Serial3 and which ones are Protocol 1 and which ones are Protocol 2. I probably should clean it up some to remove a lot of unused defines…

The code defaults to a baud rate of 1000000, but after the scan ends you can enter a different baud rate to start a new scan. Again nothing special.

I plugged in two Hexapod legs that I have setup, one using AX-12 servos and one setup with XL430-250-t… Hopefully in the not to distant future will have full hexapod with the new servos…

CM9.04 Find Servos program

Search for all servos at baud rate: 1000000
Begin Searching on Port: 1
  Begin Protocol 1:
    14 - 475
    16 - 414
    18 - 584
  Done
  Begin Protocol 2:
    8, Model:4248 - 2469
    10, Model:42410 - 2694
    12, Model:42412 - 1925
  Done
Begin Searching on Port: 3
  Begin Protocol 1:
  Done
  Begin Protocol 2:
  Done

Find Again, enter new baud or just hit enter

The Arduino Sketch code:

#include <DynamixelSDK.h>


//====================================================================================================
    // Kurts Test program to try out different ways to manipulate the AX12 servos on the PhantomX
    // This is a test, only a test...
    //
    // This version for Robotis OpenCM9.04
    //====================================================================================================
    //============================================================================
    // Global Include files
    //=============================================================================

    //=============================================================================
    // Options...
    //=============================================================================
    #if defined(__OPENCR__) || defined(__OPENCM904__)
    uint8_t port_handler_numbers[] = {1, 3};    // Setup to handle both ports of openCR
    #endif
    #if defined(TEENSYDUINO)
    uint8_t port_handler_numbers[] = {1};     // Default to Serial1
    #if defined(__MK66FX1M0__)
    // Try with the T3.6 board
    #define SERVOBUS Serial1
    #define SERVO_RX_PIN           27
    #define SERVO_TX_PIN           26
    #define SERVO_DIRECTION_PIN 28
    #define SERVO_POWER_ENABLE_PIN  29
    #else
    #define SERVO_POWER_ENABLE_PIN 2
    #endif
    #endif
    #define COUNT_PORTHANDLERS  sizeof(port_handler_numbers)

    //=============================================================================
    // Define differnt robots..
    //=============================================================================

    // Protocol version
    #define PROTOCOL_VERSION                1.0                 // See which protocol version is used in the Dynamixel
    #define PROTOCOL_VERSION2                2.0                 // See which protocol version is used in the Dynamixel
    #define DEVICENAME                      "3"                 // Check which port is being used on your controller
    #define DXL_BAUDRATE 1000000
    /** EEPROM AREA **/
    #define AX_MODEL_NUMBER_L           0
    #define AX_MODEL_NUMBER_H           1
    #define AX_VERSION                  2
    #define AX_ID                       3
    #define AX_BAUD_RATE                4
    #define AX_RETURN_DELAY_TIME        5
    #define AX_CW_ANGLE_LIMIT_L         6
    #define AX_CW_ANGLE_LIMIT_H         7
    #define AX_CCW_ANGLE_LIMIT_L        8
    #define AX_CCW_ANGLE_LIMIT_H        9
    #define AX_SYSTEM_DATA2             10
    #define AX_LIMIT_TEMPERATURE        11
    #define AX_DOWN_LIMIT_VOLTAGE       12
    #define AX_UP_LIMIT_VOLTAGE         13
    #define AX_MAX_TORQUE_L             14
    #define AX_MAX_TORQUE_H             15
    #define AX_RETURN_LEVEL             16
    #define AX_ALARM_LED                17
    #define AX_ALARM_SHUTDOWN           18
    #define AX_OPERATING_MODE           19
    #define AX_DOWN_CALIBRATION_L       20
    #define AX_DOWN_CALIBRATION_H       21
    #define AX_UP_CALIBRATION_L         22
    #define AX_UP_CALIBRATION_H         23
    /** RAM AREA **/
    #define AX_TORQUE_ENABLE            24
    #define AX_LED                      25
    #define AX_CW_COMPLIANCE_MARGIN     26
    #define AX_CCW_COMPLIANCE_MARGIN    27
    #define AX_CW_COMPLIANCE_SLOPE      28
    #define AX_CCW_COMPLIANCE_SLOPE     29
    #define AX_GOAL_POSITION_L          30
    #define AX_GOAL_POSITION_H          31
    #define AX_GOAL_SPEED_L             32
    #define AX_GOAL_SPEED_H             33
    #define AX_TORQUE_LIMIT_L           34
    #define AX_TORQUE_LIMIT_H           35
    #define AX_PRESENT_POSITION_L       36
    #define AX_PRESENT_POSITION_H       37
    #define AX_PRESENT_SPEED_L          38
    #define AX_PRESENT_SPEED_H          39
    #define AX_PRESENT_LOAD_L           40
    #define AX_PRESENT_LOAD_H           41
    #define AX_PRESENT_VOLTAGE          42
    #define AX_PRESENT_TEMPERATURE      43
    #define AX_REGISTERED_INSTRUCTION   44
    #define AX_PAUSE_TIME               45
    #define AX_MOVING                   46
    #define AX_LOCK                     47
    #define AX_PUNCH_L                  48
    #define AX_PUNCH_H                  49
    //=============================================================
    // Defines for X series (XL430)
    //=============================================================================
    #define DXL_PING                    1
    #define DXL_READ_DATA               2
    #define DXL_WRITE_DATA              3
    #define DXL_REG_WRITE               4
    #define DXL_ACTION                  5
    #define DXL_RESET                   6
    #define DXL_SYNC_READ               0x82
    #define DXL_SYNC_WRITE              0x83


    /** EEPROM AREA **/
    #define DXL_X_MODEL_NUMBER         0 //2 1060(xl430-250)
    #define DXL_X_MODEL_INFORMATION    2 //4
    #define DXL_X_VERSION              6
    #define DXL_X_ID                   7   // 1 1
    #define DXL_X_BAUD_RATE            8   // 1 1
    #define DXL_X_RETURN_DELAY_TIME    9   // 1 250
    #define DXL_X_DRIVE_MODE           10  // 1 0
    #define DXL_X_OPERATING_MODE       11  // 1 3
    #define DXL_X_SECONDARY_ID         12  // 1 255
    #define DXL_X_PROTOCOL_VERSION     13  // 1 - 2
    #define DXL_X_HOMING_OFFSET        20  // 4 - 0
    #define DXL_X_MOVING_THRESHOLD     24  // 4 - 10
    #define DXL_X_TEMPERATURE_LIMIT    31  //1 - 72
    #define DXL_X_MAX_VOLTAGE_LIMIT    32  // 2 140
    #define DXL_X_MIN_VOLTAGE_LIMIT    34  // 2 60
    #define DXL_X_PWM_LIMIT            36  // 2 885
    #define DXL_X_ACCELERATION_LIMIT   40  // 4 32767
    #define DXL_X_VElOCITY_LIMIT       44  // 4 415
    #define DXL_X_MAX_POSITION_LIMIT   48  // 4 4095
    #define DXL_X_MIN_POSITION_LIMIT   52  // 4 0
    #define DXL_X_SHUTDOWN             63  // 1 52

    /** RAM AREA **/
    #define DXL_X_TORQUE_ENABLE        64  // 1 0
    #define DXL_X_LED                  65  // 1 0
    #define DXL_X_STATUS_RETURN_LEVEL  68  // 1 2
    #define DXL_X_REGISTERED_INSTRUCTION 69 //1 (R)
    #define DXL_X_HARDWARE_ERROR_STATUS  70 // 1 (R)
    #define DXL_X_VELOCITY_I_GAIN      76  // 2 1000
    #define DXL_X_VELOCITY_P_GAIN      78  // 2 100
    #define DXL_X_POSITION_D_GAIN      80  // 2 4000
    #define DXL_X_POSITION_I_GAIN      82  // 2 0
    #define DXL_X_POSITION_P_GAIN      84  // 2 640
    #define DXL_X_FEEDFORWARD_2_GAIN   88  // 2 0
    #define DXL_X_FEEDFORWARD_1_GAIN   90  // 2 0
    #define DXL_X_BUS_WATCHDOG         98  // 1 -
    #define DXL_X_GOAL_PWM             100 // 2
    #define DXL_X_GOAL_VELOCITY        104 // 4
    #define DXL_X_PROFILE_ACCELERATION 108 // 4
    #define DXL_X_PROFILE_VELOCITY     112 // 4
    #define DXL_X_GOAL_POSITION        116 // 4
    #define DXL_X_REALTIME_TICK        120 // 2 (R)
    #define DXL_X_MOVING               122 // 1 (R)
    #define DXL_X_MOVING_STATUS        123 // 1 (R)
    #define DXL_X_PRESENT_PWN          124 // 2 (R)
    #define DXL_X_PRESENT_LOAD         126 // 2 (R)
    #define DXL_X_PRESENT_VELOCITY     128 // 4 (R)
    #define DXL_X_PRESENT_POSITION     132 // 4 (R)
    #define DXL_X_VELOCITY_TRAJECTORY  136 // 4 (R)
    #define DXL_X_POSITION_TRAJECTORY  140 // 4 (R)
    #define DXL_X_PRESENT_INPUT_VOLTAGE  144 // 2 (R)
    #define DXL_X_PRESENT_TEMPERATURE  146 // 1 (R)



    //=============================================================================
    // Globals
    //=============================================================================
    // Global objects
    // Handle to port handler and packet handler;
    dynamixel::PortHandler *portHandlers[COUNT_PORTHANDLERS];

    dynamixel::PacketHandler *packetHandler1;
    dynamixel::PacketHandler *packetHandler2;
    uint8_t g_servo_protocol[254];
    uint32_t g_baud_rate = DXL_BAUDRATE;
    //====================================================================================================
    // Setup
    //====================================================================================================
    void setup() {

      while (!Serial && (millis() < 3000)) ;  // Give time for Teensy and other USB arduinos to create serial port
      Serial.begin(38400);  // start off the serial port.
      Serial.println("nCM9.04 Find Servos program");

    #if defined(SERVO_RX_PIN)
      SERVOBUS.setRX(SERVO_RX_PIN);
    #endif
    #if defined(SERVO_TX_PIN)
      SERVOBUS.setTX(SERVO_TX_PIN);
    #endif

    #ifdef SERVO_POWER_ENABLE_PIN
      pinMode(SERVO_POWER_ENABLE_PIN, OUTPUT);
      digitalWrite(SERVO_POWER_ENABLE_PIN, HIGH);
    #endif
      pinMode(4, OUTPUT);

      // Initialize PacketHandler instance
      packetHandler1 = dynamixel::PacketHandler::getPacketHandler(PROTOCOL_VERSION);
      packetHandler2 = dynamixel::PacketHandler::getPacketHandler(PROTOCOL_VERSION2);
      // Open port

      // Set the protocol version
      // Get methods and members of Protocol1PacketHandler or Protocol2PacketHandler
      // Initialize PortHandler instances

      char port_string[5];
      for (uint8_t i = 0; i < COUNT_PORTHANDLERS; i++) {
        itoa(port_handler_numbers[i], port_string, sizeof(port_string));
        portHandlers[i] = dynamixel::PortHandler::getPortHandler(port_string);

        // Lets init the two different port handlers.
    #if defined(SERVO_DIRECTION_PIN)
        portHandlers[i]->setTXEnablePin(SERVO_DIRECTION_PIN);
    #endif
        if (!portHandlers[i]->openPort()) {
          Serial.print("Failed to open port 1 the Dynamixel port: ");
          Serial.println(port_string);
        }
        if (!portHandlers[i]->setBaudRate(g_baud_rate)) {
          Serial.print("Failed to change the Port 1 Dynamixel baudrate: ");
          Serial.println(port_string);
        }
      }
      delay(1000);
      // Lets start of trying to locate all servos.
      FindServos();


    }


    //====================================================================================================
    // Loop
    //====================================================================================================
    void loop() {
      // Output a prompt

      // lets toss any charcters that are in the input queue
      Serial.println("nFind Again, enter new baud or just hit enter");
      Serial.flush();  // make sure the complete set of prompts has been output...
      while (Serial.read() != -1) ; // Remove any stuff still in serial buffer.
      // Get a command
      uint32_t new_baud = CheckForNewBaud();
      if (new_baud) {
        SetBaudRate(new_baud);
      }
      FindServos();
    }

    //=======================================================================================

    void FindServos(void) {

      uint16_t w;
      Serial.print("nSearch for all servos at baud rate: ");
      Serial.println(g_baud_rate, DEC);

      // Initialize to no servos...
      for (int i = 0; i < 254; i++) {
        g_servo_protocol[i] = 0; // not found
      }
      for (uint8_t port_index = 0; port_index < COUNT_PORTHANDLERS; port_index++) {

        dynamixel::PortHandler *portHandler = portHandlers[port_index];
        Serial.print("Begin Searching on Port: ");
        Serial.println(port_handler_numbers[port_index], DEC);

        Serial.println("  Begin Protocol 1: ");
        for (int i = 0; i < 254; i++) {
          if (packetHandler1->read2ByteTxRx(portHandler, i, AX_PRESENT_POSITION_L, &w) == COMM_SUCCESS) {
            if (g_servo_protocol[i]) {
              Serial.println("Multiple servos found with same ID");
            }
            g_servo_protocol[i] = 1;
            Serial.print("    ");
            Serial.print(i, DEC);
            Serial.print(" - ");
            Serial.println(w, DEC);
          }
        }

        Serial.println("  Done");
        Serial.println("  Begin Protocol 2: ");
        for (int i = 0; i < 254; i++) {
          uint16_t model_number;
          uint32_t position;
          if (packetHandler2->ping(portHandler, i, &model_number) == COMM_SUCCESS) {
            if (g_servo_protocol[i]) {
              Serial.println("Multiple servos found with same ID");
            }
            g_servo_protocol[i] = 2;
            Serial.print("    ");
            Serial.print(i, DEC);
            Serial.print(", Model:");
            Serial.print(model_number, HEX);
            packetHandler2->read4ByteTxRx(portHandler, i, DXL_X_PRESENT_POSITION, &position);
            Serial.print(i, DEC);
            Serial.print(" - ");
            Serial.println(position, DEC);
          }
        }
        Serial.println("  Done");
      }
    }

    //====================================================================================================
    // Process command line, optionally return new baud rate
    uint32_t CheckForNewBaud(void) {
      int ch;
      uint32_t new_baud = 0;

      for (;;) {
        // throw away any thing less than CR character...
        ch = Serial.read();
        if (ch != -1) {
          if ((ch >= '0') && (ch <= '9')) {
            new_baud = new_baud * 10 + (uint32_t)(ch - '0');
          } else {
            break;
          }
        }
      }
      while (Serial.read() != -1) ; // remove any trailing stuff.
      return new_baud;
    }


    //=======================================================================================
    void SetBaudRate(uint32_t new_baud)
    {
      Serial.print("Setting Baud to: ");
      Serial.println(new_baud);

      for (uint8_t i = 0; i < COUNT_PORTHANDLERS; i++) {
        if (!portHandlers[i]->setBaudRate(new_baud)) {
          Serial.print("Failed to change the Port ");
          Serial.print(port_handler_numbers[i], DEC);
          Serial.print("Baud to: ");
          Serial.println(new_baud, DEC);
        }
      }
      g_baud_rate = new_baud;
    }

I also put it up in my WIP branch of my Open CM project on github: https://github.com/KurtE/Open_CM904/tree/WIP_Support_XL320_Servos

I also put up a similar post up on Robotis in the thread: ROBOTIS

2 Likes

Thank you for sharing it in RobotSource.
This will be very useful for the users who wants to stick to the DynamixelSDK.

Thanks Will,

When you say that it will be useful for the users who want to stick with the DynamixelSDK, are you suggesting that there is another library that Robotis suggests for users to use for their projects? Like the Dynamixel Workbench?

So far I personally prefer using the SDK as it gives me more exact control over all of the servos and is more efficient, but I can understand that the Workbench might be good to abstract away some of the differences between servos. Things like to set the Goal position of a servo the Register # changes between different servos as well as the number of bytes and for example has the ability to convert the angle into servo units, which is great…

But in the current state I don’t think the Workbench will work on my PhantomX hexapods as it appears like the library is currently setup to handle maybe a max of 14 servos on the OpenCM and OpenCR boards and 3 DOF hexapods have at least 18 servos and some Hexapods are 4 DOF so they would require at least 24.

Likewise currently I don’t believe enough of the Registers are supported. Things like current voltage, temperature… But may have to take another look!

Again Thanks

Kurt,

Well, in most case I recommend to use DynamixelSDK for development for the same reason you have.
DynamixelWorkbench also provides very handy features such as auto detecting Dynamixel ID, protocol version and other information.
So I sometimes recommend to use DynamixelWorkbench to users who want to check their Dynamixel and beginners as a substitute for R+Manager in Linux.
As you mentioned, Workbench has limitations to be used for serious projects.
Good luck and have fun with your project :slight_smile: