Dynamixels AX-12A on OpenCM 9.04 + OpenCM 485: how to send instruction packets

[Daisuke71]

Hello everyone!

I am trying to write an Arduino sketch to send instruction packets to a Dynamixel AX-12A connected to an OpenCM 9.04 + OpenCM 485 board.

I am doing this for learning purposes, even if from a certain point of view is quite "reinventing the wheel".

I wrote the following code studying several examples and the Robotis references, but I am not able to get it working.

The basic idea is to use the REG_WRITE and ACTION instructions to communicate with the AX-12A, in order to first store the goal position in the proper register and afterwards trigger the motion of the servo.
However, I have not been able to find in the documentation the proper pin number to communicate with (this assuming that the pin number is the error and not something else!).

I would really appreciate if someone can help me spotting where the problem is!
Thankyou!

// Includes DynamixelWorkbench
#include <DynamixelWorkbench.h>

// Defines communication port with OpenCM 9.04 and OpenCM 485
  #if defined(__OPENCM904__)
    #define DEVICE_NAME "3" 
  #endif

// Defines Baud rate for Dynamixel AX-12A
  #define BAUDRATE  1000000
  
// Defines dxl_wb as a DynamixelWorkbench object
  DynamixelWorkbench dxl_wb;

// Defines the variables for Goal Position, Servo ID and Servo model
  int32_t servo_goal_position;
  uint8_t servo_id = 5;
  uint16_t model_number = 12;

// Declares a boolean variable to check the outcome of setup cycle and stop the program if anything fails
  bool SetupError = false;

// HERE COMES THE ISSUE: WHICH PIN IS TO BE USED IN pinMode AND digitalWrite INSTRUCTIONS TO COMMUNICATE WITH DYNAMIXELS VIA OpenCM9.04+OpenCM485?
// HERE BELOW I SET IT TO 26 BUT IT DOES NOT WORK. THE SAME HAPPENS WITH OTHER VALUES.
uint8_t OPENCM_TX_PIN = 26;


void setup()
{
  // Declares a boolean variable to check the outcome of certain actions
    bool result = false;

  // Declares a pointer to a char array containing error codes etc.
    const char *log;

  // Activates serial monitor
    Serial.begin(57600);
    while(!Serial);

  // Sets Pin Mode for communication with Dynamixels
    pinMode(OPENCM_TX_PIN,OUTPUT);

  // Initiate DEVICE_NAME (3 is OpenCM 485) at BAUDRATE (1Mbps)
    result = dxl_wb.init(DEVICE_NAME, BAUDRATE, &log);
    if (result == false)
      {
        SetupError = true;
        Serial.println(log);
        Serial.println("FAILED to init device " + String(DEVICE_NAME));
        Serial.println("Stop program execution");
        return;
      }
    else
      {
        Serial.println("Succeeded to init device " + String(DEVICE_NAME));
        Serial.println("Baud Rate is " + String(BAUDRATE));  
      }

  // Pings the motor and activates Joint Mode
    result = dxl_wb.ping(servo_id, &model_number, &log);
        if (result == false)
          {
            SetupError = true;
            Serial.println(log);
            Serial.println("FAILED to ping Servo " + String(servo_id));
            Serial.println("Stop program execution");
            return;
          }
        else
          {
            Serial.println("Succeeded to ping Servo " + String(servo_id));
            Serial.println("Setting Joint Mode for Servo " + String(servo_id));
            result = dxl_wb.jointMode(servo_id, 0, 0, &log);
            if (result == false)
              {
                SetupError = true;
                Serial.println(log);
                Serial.println("FAILED to set Joint Mode for Servo " + String(servo_id));
                Serial.println("Stop program execution");
                return;
              }
            else
              {
                Serial.println("Succeeded to set Joint Mode for Servo " + String(servo_id));
              }
          }

// Setup complete
}

void loop()
{
// Main loop

  // Check if setup completed correctly
  if (SetupError == true)
  {
    // Setup ended with errors, stops program execution
    return;
  }
  
  if (Serial.available()) // Waits for user input
  {
    // Reads input
      String Serial_Input = Serial.readStringUntil('\n'); // Reads the string until CR
      Serial_Input.trim(); // Cleanups the string
      servo_goal_position = atoi(Serial_Input.c_str()); // Converts the string in a number
      Serial.println("[Position]: " + String(servo_goal_position)); // Print on serial monitor the input received

    // Writes the goal position to the servo register (RE_WRITE command)
      Serial.println("TestWriteGoalPosition");
      TestWriteGoalPosition(servo_id, servo_goal_position);

    // Activates the servo (ACTION command)
      Serial.println("TestActivateDynamixels");
      TestActivateDynamixels(servo_id);
  }
}

void TestWriteGoalPosition(uint8_t servo_id, int32_t servo_goal_position)
/*****************************************************************************************************************************************************
    Serial3.write(0xFF);        // Header 1
    Serial3.write(0xFF);        // Header 2
    Serial3.write(servo_id);    // ID of the servo, 0xFE is Broadcast ID
    Serial3.write(lenght);      // Lenght of data packet (number of parameters + 2)
    Serial3.write(0x04);        // REG_WRITE Instruction
    Serial3.write(data);        // Parameter 1: Register address
    Serial3.write(data);        // Parameter 2: low byte of the value to be written in the register
    Serial3.write(data);        // Parameter 3: high byte of the value to be written in the register
    Serial3.write(Checksum);    // Checksum (~(servoid+lenght+instruction+Parameter 1+...+Parameter n))
******************************************************************************************************************************************************/
  {
  Serial.println("Extracts high and low bytes");
    // Extracts high byte
      char servo_goal_position_H = highByte(servo_goal_position);
    // Extracts low byte
      char servo_goal_position_L = lowByte(servo_goal_position);

  Serial.println("Computes checksum for REG_WRITE");
    // Computes Checksum according to Robotis Protocol 1.0
      byte Checksum = ~(servo_id + 0x05 + 0x04 + 0x1E + servo_goal_position_L + servo_goal_position_H);

  Serial.println("Transmits REG_WRITE command");
    // Activates transmission
      digitalWrite(OPENCM_TX_PIN,HIGH);
    // Transmits on Serial3 (OpenCM 485) the instruction
      Serial3.write(0xFF);                    // Header 1
      Serial3.write(0xFF);                    // Header 2
      Serial3.write(servo_id);                // Servo ID
      Serial3.write(0x05);                    // Packet lenght
      Serial3.write(0x04);                    // REG_WRITE Instruction
      Serial3.write(0x1E);                    // Parameter 1: Register address for Goal Position is 30 (0x1E)
      Serial3.write(servo_goal_position_L);   // Parameter 2: Goal Position, low bite
      Serial3.write(servo_goal_position_H);   // Parameter 3: Goal Position, high bite
      Serial3.write(Checksum);                // Checksum

  Serial.println("Flush");
    // Waits completion of the transmission
      Serial3.flush();
      
  Serial.println("Reads status packet");
    // Activates receipt
      digitalWrite(OPENCM_TX_PIN,LOW);
    // Reads status packet and prints it on serial monitor
      while(Serial3.available())
        {
          Serial.println(Serial3.read());
        }    
  }

void TestActivateDynamixels(uint8_t servo_id)
/*****************************************************************************************************************************************************
    Serial3.write(0xFF);        // Header 1
    Serial3.write(0xFF);        // Header 2
    Serial3.write(servo_id);    // ID of the servo, 0xFE is Broadcast ID
    Serial3.write(lunghezza);   // Lenght of data packet (number of parameters + 2)
    Serial3.write(0x05);        // ACTION instruction
    Serial3.write(Checksum);    // Checksum (~(ID+Lunghezza+Istruzione+Parametro 1+...+Parametro n))
******************************************************************************************************************************************************/
  {
  Serial.println("Computes checksum for ACTION");
    // Computes Checksum according to Robotis Protocol 1.0
      byte Checksum = ~(servo_id + 0x02 + 0x05);

  Serial.println("Transmits ACTION command");
    // Activates transmission
      digitalWrite(OPENCM_TX_PIN,HIGH);
    // Transmits on Serial3 (OpenCM 485) the instruction
      Serial3.write(0xFF);                    // Header 1
      Serial3.write(0xFF);                    // Header 2
      Serial3.write(servo_id);                // Servo ID
      Serial3.write(0x02);                    // Packet lenght
      Serial3.write(0x05);                    // ACTION instruction
      Serial3.write(Checksum);                // Checksum

  Serial.println("Flush");
    // Waits completion of the transmission
      Serial3.flush();

  Serial.println("Reads status packet");
    // Activates receipt
      digitalWrite(OPENCM_TX_PIN,LOW);
    // Reads status packet and prints it on serial monitor
      while(Serial3.available())
        {
          Serial.println(Serial3.read());
        }
  }

[jwatte]

Here's my setup code for OpenCM 9.04:

void DxlIO::begin() {  afio_remap(AFIO_REMAP_USART1);
  afio_cfg_debug_ports(AFIO_DEBUG_FULL_SWJ_NO_NJRST);
  gpio_set_mode(PORT_TXRX_DIRECTION, PIN_TXRX_DIRECTION, GPIO_OUTPUT_PP);
  gpio_write_bit(PORT_TXRX_DIRECTION, PIN_TXRX_DIRECTION, 0 );// RX Enable
  gpio_set_mode(PORT_DXL_TXD, PIN_DXL_TXD, GPIO_AF_OUTPUT_PP);
  gpio_set_mode(PORT_DXL_RXD, PIN_DXL_RXD, GPIO_INPUT_PU);
  usart_init(USART1);
  nvic_irq_set_priority(USART1->irq_num, 0);
  usart_set_baud_rate(USART1, STM32_PCLK2, 2000000);  //  2 Mbit
  usart_enable(USART1);
  DXL_RXD;
}

This still uses the <dxl_constants.h> and <Dynamixel.h> headers from the IDE to define the correct pin numbers.

Note that the OpenCM 9.04 already has the right single-duplex driver chip for the AX12 on board; you don't need the 485 board for this. If you're seeing that "nothing works," then perhaps the problem is in the connections to the board instead?
Do you have an oscilloscope or logic analyzer hooked up to see what's going on?

[KurtEck]

I am sort of confused here. If you are using Dynamixel SDK or Workbench it should handle everything for you. In that you should be able to send or receive packets using built in functions to these libraries...

This is true for either Serial1 or Serial3... If you are wanting to use Serial3 and the OpenCM 485 expansion board pins....

But if you wish to do it for yourself, I suggest you look at the sources for the Dynamixel SDK that is part of the Arduino install.
In particular protocol1_packet_handler.cpp and port_handler_arduino.cpp

So if you look at txPacket and what it does.

Is it first clears out the RX side. Something like:
while (Serial3.available() Serial3.read();

It then calls off to int PortHandlerArduino::writePort(uint8_t *packet, int length)
Which then calls off to setTxEnable();

Which tells the system to put the half duplex stuff associated with the Serial port in this case Serial3 into write Mode.
In which for Serial3 translates to something like digitalWrite(D22, HIGH);

You then output your data to the serial port in your case Serial3.

You wait until the data is fully transmitted. In this case Serial3.flush();

And then you change the Serial port to RX mode (setTxDisable) which does digitalWrite(D22, LOW);

As for Serial1 Not sure if the IO pin associated with Tx/RX is in their pin table or not...

[Daisuke71]

Hi,

First of all thanks for your answers, I really appreciate them as well as the code suggestions that helped a lot.

@jwatte
The OpenCM 9.04 I am using is a c-type equipped with the connectors for the XL-320 servos.
I am currently using the board to control a Bioloid (18 AXs) so I preferred to attach it to a 485 instead of de-soldering the small connectors and swap them with the AX ones, just to keep the capability to drive some XLs also.
I do not have an oscilloscope, however I can tell you that the connection works fine.
Using the standard Dynamixel Workbench instructions I am able to drive the motors as expected, but also using the code I originally posted (now that I corrected it) everything works.

@KurtEck
Definitely true, the built-in functions contain in principle everything I need and I already wrote a working sketch to drive my servos using them.
The reasons for trying to write this code are twofold. On one side, I interested in better understanding how do they work.
On the other side, I was trying to update the goal position in some of the servos without triggering the motion.
However, I have not been able to find the right instruction to activate the motors (the equivalend of the "action" packet) in the Dynamixel Workbench or SDK. For sure I did not read properly the documentation, but anyway I was trying to write myself the (apparently) missing function.
In the meantime however you gave me the solution: digitalWrite(D22, HIGH);.
Actually using pin 22 the code works as expected.
This was definitely not clear to me when reading the OpenCM 9.04 emanual. I will check the functions that you suggested (that seems not to be present in my Arduino installation - I will look for them on the web).

Cheers,
Luca

[Le0nX]

Hi! I've got the same problem with my opencm 9.04 + EXP + ax-12a. But I can't understand why do you need D22 pin... TX3 is on D24(PB10)...

[KurtEck]

Look at the Expansion board schematic: http://support.robotis.com/en/baggag...ncm_485exp.pdf

You will see that the TX3/RX3 connect to DXL buss through some level shifter parts U2/U3, which use an IO pin to control if the DXL TTL pin is either doing input or output.