Results 1 to 4 of 4

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

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

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

  2. #2

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

    Here's my setup code for OpenCM 9.04:

    Code:
    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?

  3. #3

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

    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...

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

    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

Thread Information

Users Browsing this Thread

There are currently 1 users browsing this thread. (0 members and 1 guests)

Similar Threads

  1. OpenCM IDE vs Arduino IDE
    By cod65 in forum DYNAMIXEL & Robot Actuators
    Replies: 4
    Last Post: 03-19-2018, 01:14 PM
  2. OpenCM 9.04 iwdg
    By lpala in forum Arbotix, Microcontrollers, Arduino
    Replies: 0
    Last Post: 07-28-2017, 01:32 PM
  3. OpenCM 9.04, best way to beep?
    By jwatte in forum DYNAMIXEL & Robot Actuators
    Replies: 10
    Last Post: 04-09-2017, 01:11 PM
  4. OpenCM IDE (Ubuntu 14.04)
    By rasheinstein in forum Arbotix, Microcontrollers, Arduino
    Replies: 2
    Last Post: 04-12-2016, 01:47 PM
  5. Question(s) MX-64AR not responding to instruction packets
    By kammce in forum DYNAMIXEL & Robot Actuators
    Replies: 7
    Last Post: 04-02-2015, 12:40 PM

Tags for this Thread

Posting Permissions

  • You may not post new threads
  • You may not post replies
  • You may not post attachments
  • You may not edit your posts
  •