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!


Code:
// 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());
        }
  }