ROBOTIS-GIT / DynamixelSDK

ROBOTIS Dynamixel SDK (Protocol1.0/2.0)
http://emanual.robotis.com/docs/en/software/dynamixel/dynamixel_sdk/overview/
Apache License 2.0
454 stars 405 forks source link

indirect_address example works with XM540 but not XL330? #502

Open ottensmeyer opened 3 years ago

ottensmeyer commented 3 years ago

ISSUE TEMPLATE ver. 1.2.0

Please fill this template for more accurate and prompt support.

  1. Which DYNAMIXEL SDK version do you use?

    • 3.7.31 (?) - was installed with OpenCR package for arduino around Jan, 2021
  2. Which programming language/tool do you use?

    • Arduino IDE 1.8.13
  3. Which operating system do you use?

    • Windows 10
  4. Which USB serial converter do you use?

    • OpenCR
  5. Which DYNAMIXEL do you use?

    • XM540-W150-T, XL330-M077-T
  6. Have you searched the issue from the closed issue threads?

    • yes
  7. Please describe the issue in detail

    • I am trying to use the indirect addressing example with the XM540 and XL330 servos (Protocol 2.0). With the XM540, I change the address values in the #defines, change the position start/end locations to reasonable values (0, 1023), update the LED values (from 255 to 1), and I correctly read the position and moving state values

    • When I do the same for the XL330, the position values are correctly read out, but the moving state never changes from 0. I've replaced the moving state address (122) to other 1-byte addresses (e.g. temperature (146), LED state (65)), which also only read 0.

    • Writing using indirect address works fine - the goal_position and LED commands are accomplished successfully

    • Am I doing something wrong in using indirect addressing? The example is for the Dynamixel PRO 54-200, and USB2DYNAMIXEL, and the example works correctly for the XM540 and OpenCR, but not the XL330

    • I've confirmed using the DYNAMIXEL Wizard 2.0 that the moving, temperature and other states are reading correctly from the XL330, which has address values for each of these the same as what's on the eManual (the Wizard also shows that the XL330 is not in need of an update)

    • Typical output using XM540 - PresPos is correctly reported, and IsMoving output shows the correct temperature (about 28) using the code below:

Start..
Succeeded to open the port!
Succeeded to change the baudrate!
Dynamixel has been successfully connected 
Press any key to continue! (or press q to quit!)
[ID:1] GoalPos:0  PresPos:371  IsMoving:28 
[ID:1] GoalPos:0  PresPos:364  IsMoving:28 
[ID:1] GoalPos:0  PresPos:346  IsMoving:28 
[ID:1] GoalPos:0  PresPos:324  IsMoving:28 
[ID:1] GoalPos:0  PresPos:302  IsMoving:28 
[ID:1] GoalPos:0  PresPos:280  IsMoving:28 
...
[ID:1] GoalPos:0  PresPos:34  IsMoving:28 
[ID:1] GoalPos:0  PresPos:28  IsMoving:28 
[ID:1] GoalPos:0  PresPos:23  IsMoving:28 
Press any key to continue! (or press q to quit!)
  1. How can we reproduce the issue?
    • create a custom cable that draws 5V supply from the 5V connector on the OpenCR, (because the XL330 doesn't take 12V supply according to the spec sheet) and uses the ground and TTL signals from the TTL connector on the OpenCR and connect to an XL330-M077-T servo programmed to ID 1
    • try the attached code and watch to see whether the temperature reading shows something different from 0, and is reasonable (i.e. something a few degrees C above room temperature)
    • use a standard TTL cable and connect to an XM540 motor, comment out the address values for the XL330, uncomment the address values for XM540 (see comments in the code), which should output about 28 (degrees C, shown in the output after "IsMoving")
/*******************************************************************************
* Copyright (c) 2016, ROBOTIS CO., LTD.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* * Redistributions of source code must retain the above copyright notice, this
*   list of conditions and the following disclaimer.
*
* * Redistributions in binary form must reproduce the above copyright notice,
*   this list of conditions and the following disclaimer in the documentation
*   and/or other materials provided with the distribution.
*
* * Neither the name of ROBOTIS nor the names of its
*   contributors may be used to endorse or promote products derived from
*   this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*******************************************************************************/

/* Author: Ryu Woon Jung (Leon) */

//
// *********     Indirect Address Example      *********
//
//
// Available Dynamixel model on this example : All models using Protocol 2.0
// This example is tested with a Dynamixel PRO 54-200, and an USB2DYNAMIXEL
// Be sure that Dynamixel PRO properties are already set as %% ID : 1 / Baudnum : 1 (Baudrate : 57600)
//

#include <DynamixelSDK.h>

// Control table address
// Control table address is different in Dynamixel model
#define ADDR_PRO_INDIRECTADDRESS_FOR_WRITE      168                  // EEPROM region
#define ADDR_PRO_INDIRECTADDRESS_FOR_READ       178                  // EEPROM region
#define ADDR_PRO_TORQUE_ENABLE                  64
#define ADDR_PRO_LED_RED                        65
#define ADDR_PRO_GOAL_POSITION                  116
//#define ADDR_PRO_MOVING                         122
#define ADDR_PRO_MOVING                         146 // temperature replaces moving state
#define ADDR_PRO_PRESENT_POSITION               132
#define ADDR_PRO_INDIRECTDATA_FOR_WRITE         208 // for XL330 servo
#define ADDR_PRO_INDIRECTDATA_FOR_READ          213 // for XL330 servo
//#define ADDR_PRO_INDIRECTDATA_FOR_WRITE         224  // for XM540 servo
//#define ADDR_PRO_INDIRECTDATA_FOR_READ          229  // for XM540 servo

// Data Byte Length
#define LEN_PRO_LED_RED                         1
#define LEN_PRO_GOAL_POSITION                   4
#define LEN_PRO_MOVING                          1
#define LEN_PRO_PRESENT_POSITION                4
#define LEN_PRO_INDIRECTDATA_FOR_WRITE          5
#define LEN_PRO_INDIRECTDATA_FOR_READ           5

// Protocol version
#define PROTOCOL_VERSION                        2.0                 // See which protocol version is used in the Dynamixel

// Default setting
#define DXL_ID                                  1                   // Dynamixel ID: 1
#define BAUDRATE                                57600
#define DEVICENAME                              "OpenCR_DXL_Port"   // This definition only has a symbolic meaning and does not affect to any functionality

#define TORQUE_ENABLE                           1                   // Value for enabling the torque
#define TORQUE_DISABLE                          0                   // Value for disabling the torque
#define DXL_MINIMUM_POSITION_VALUE             0              // Dynamixel will rotate between this value
#define DXL_MAXIMUM_POSITION_VALUE              1023              // and this value (note that the Dynamixel would not move when the position value is out of movable range. Check e-manual about the range of the Dynamixel you use.)
#define DXL_MINIMUM_LED_VALUE                   0                   // Dynamixel LED will light between this value
#define DXL_MAXIMUM_LED_VALUE                   1                 // and this value
#define DXL_MOVING_STATUS_THRESHOLD             25                  // Dynamixel moving status threshold

#define ESC_ASCII_VALUE                         0x1b

#define CMD_SERIAL                              Serial

int getch()
{
  while(1)
  {
    if( CMD_SERIAL.available() > 0 )
    {
      break;
    }
  }

  return CMD_SERIAL.read();
}

int kbhit(void)
{
  return CMD_SERIAL.available();
}

void setup()
{
  Serial.begin(115200);
  while(!Serial);

  Serial.println("Start..");

  // 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);

  // Initialize GroupSyncWrite instance
  dynamixel::GroupSyncWrite groupSyncWrite(portHandler, packetHandler, ADDR_PRO_INDIRECTDATA_FOR_WRITE, LEN_PRO_INDIRECTDATA_FOR_WRITE);

  // Initialize Groupsyncread instance
  dynamixel::GroupSyncRead groupSyncRead(portHandler, packetHandler, ADDR_PRO_INDIRECTDATA_FOR_READ, LEN_PRO_INDIRECTDATA_FOR_READ);

  int index = 0;
  int dxl_comm_result = COMM_TX_FAIL;             // Communication result
  bool dxl_addparam_result = false;               // addParam result
  bool dxl_getdata_result = false;                // GetParam result
  int dxl_goal_position[2] = {DXL_MINIMUM_POSITION_VALUE, DXL_MAXIMUM_POSITION_VALUE};  // Goal position

  uint8_t dxl_error = 0;                          // Dynamixel error
  uint8_t dxl_moving = 0;                         // Dynamixel moving status
  uint8_t param_indirect_data_for_write[LEN_PRO_INDIRECTDATA_FOR_WRITE];
  uint8_t dxl_led_value[2] = {0x00, 0x01};        // Dynamixel LED value
  int32_t dxl_present_position = 0;               // Present position

  // Open port
  if (portHandler->openPort())
  {
    Serial.print("Succeeded to open the port!\n");
  }
  else
  {
    Serial.print("Failed to open the port!\n");
    return;
  }

  // Set port baudrate
  if (portHandler->setBaudRate(BAUDRATE))
  {
    Serial.print("Succeeded to change the baudrate!\n");
  }
  else
  {
    Serial.print("Failed to change the baudrate!\n");
    return;
  }

  // Disable Dynamixel Torque :
  // Indirect address would not accessible when the torque is already enabled
  dxl_comm_result = packetHandler->write1ByteTxRx(portHandler, DXL_ID, ADDR_PRO_TORQUE_ENABLE, TORQUE_DISABLE, &dxl_error);
  if (dxl_comm_result != COMM_SUCCESS)
  {
    Serial.print(packetHandler->getTxRxResult(dxl_comm_result));
  }
  else if (dxl_error != 0)
  {
    Serial.print(packetHandler->getRxPacketError(dxl_error));
  }
  else
  {
    Serial.print("Dynamixel has been successfully connected \n");
  }

  // INDIRECTDATA parameter storages replace LED, goal position, present position and moving status storages
  dxl_comm_result = packetHandler->write2ByteTxRx(portHandler, DXL_ID, ADDR_PRO_INDIRECTADDRESS_FOR_WRITE + 0, ADDR_PRO_GOAL_POSITION + 0, &dxl_error);
  if (dxl_comm_result != COMM_SUCCESS)
  {
    Serial.print(packetHandler->getTxRxResult(dxl_comm_result));
  }
  else if (dxl_error != 0)
  {
    Serial.print(packetHandler->getRxPacketError(dxl_error));
  }

  dxl_comm_result = packetHandler->write2ByteTxRx(portHandler, DXL_ID, ADDR_PRO_INDIRECTADDRESS_FOR_WRITE + 2, ADDR_PRO_GOAL_POSITION + 1, &dxl_error);
  if (dxl_comm_result != COMM_SUCCESS)
  {
    Serial.print(packetHandler->getTxRxResult(dxl_comm_result));
  }
  else if (dxl_error != 0)
  {
    Serial.print(packetHandler->getRxPacketError(dxl_error));
  }

  dxl_comm_result = packetHandler->write2ByteTxRx(portHandler, DXL_ID, ADDR_PRO_INDIRECTADDRESS_FOR_WRITE + 4, ADDR_PRO_GOAL_POSITION + 2, &dxl_error);
  if (dxl_comm_result != COMM_SUCCESS)
  {
    Serial.print(packetHandler->getTxRxResult(dxl_comm_result));
  }
  else if (dxl_error != 0)
  {
    Serial.print(packetHandler->getRxPacketError(dxl_error));
  }

  dxl_comm_result = packetHandler->write2ByteTxRx(portHandler, DXL_ID, ADDR_PRO_INDIRECTADDRESS_FOR_WRITE + 6, ADDR_PRO_GOAL_POSITION + 3, &dxl_error);
  if (dxl_comm_result != COMM_SUCCESS)
  {
    Serial.print(packetHandler->getTxRxResult(dxl_comm_result));
  }
  else if (dxl_error != 0)
  {
    Serial.print(packetHandler->getRxPacketError(dxl_error));
  }

  dxl_comm_result = packetHandler->write2ByteTxRx(portHandler, DXL_ID, ADDR_PRO_INDIRECTADDRESS_FOR_WRITE + 8, ADDR_PRO_LED_RED, &dxl_error);
  if (dxl_comm_result != COMM_SUCCESS)
  {
    Serial.print(packetHandler->getTxRxResult(dxl_comm_result));
  }
  else if (dxl_error != 0)
  {
    Serial.print(packetHandler->getRxPacketError(dxl_error));
  }

  dxl_comm_result = packetHandler->write2ByteTxRx(portHandler, DXL_ID, ADDR_PRO_INDIRECTADDRESS_FOR_READ + 0, ADDR_PRO_PRESENT_POSITION + 0, &dxl_error);
  if (dxl_comm_result != COMM_SUCCESS)
  {
    Serial.print(packetHandler->getTxRxResult(dxl_comm_result));
  }
  else if (dxl_error != 0)
  {
    Serial.print(packetHandler->getRxPacketError(dxl_error));
  }

  dxl_comm_result = packetHandler->write2ByteTxRx(portHandler, DXL_ID, ADDR_PRO_INDIRECTADDRESS_FOR_READ + 2, ADDR_PRO_PRESENT_POSITION + 1, &dxl_error);
  if (dxl_comm_result != COMM_SUCCESS)
  {
    Serial.print(packetHandler->getTxRxResult(dxl_comm_result));
  }
  else if (dxl_error != 0)
  {
    Serial.print(packetHandler->getRxPacketError(dxl_error));
  }

  dxl_comm_result = packetHandler->write2ByteTxRx(portHandler, DXL_ID, ADDR_PRO_INDIRECTADDRESS_FOR_READ + 4, ADDR_PRO_PRESENT_POSITION + 2, &dxl_error);
  if (dxl_comm_result != COMM_SUCCESS)
  {
    Serial.print(packetHandler->getTxRxResult(dxl_comm_result));
  }
  else if (dxl_error != 0)
  {
    Serial.print(packetHandler->getRxPacketError(dxl_error));
  }

  dxl_comm_result = packetHandler->write2ByteTxRx(portHandler, DXL_ID, ADDR_PRO_INDIRECTADDRESS_FOR_READ + 6, ADDR_PRO_PRESENT_POSITION + 3, &dxl_error);
  if (dxl_comm_result != COMM_SUCCESS)
  {
    Serial.print(packetHandler->getTxRxResult(dxl_comm_result));
  }
  else if (dxl_error != 0)
  {
    Serial.print(packetHandler->getRxPacketError(dxl_error));
  }

  dxl_comm_result = packetHandler->write2ByteTxRx(portHandler, DXL_ID, ADDR_PRO_INDIRECTADDRESS_FOR_READ + 8, ADDR_PRO_MOVING, &dxl_error);
  if (dxl_comm_result != COMM_SUCCESS)
  {
    Serial.print(packetHandler->getTxRxResult(dxl_comm_result));
  }
  else if (dxl_error != 0)
  {
    Serial.print(packetHandler->getRxPacketError(dxl_error));
  }

  // Enable Dynamixel Torque
  dxl_comm_result = packetHandler->write1ByteTxRx(portHandler, DXL_ID, ADDR_PRO_TORQUE_ENABLE, TORQUE_ENABLE, &dxl_error);
  if (dxl_comm_result != COMM_SUCCESS)
  {
    Serial.print(packetHandler->getTxRxResult(dxl_comm_result));
  }
  else if (dxl_error != 0)
  {
    Serial.print(packetHandler->getRxPacketError(dxl_error));
  }

  // Add parameter storage for the present position value
  dxl_addparam_result = groupSyncRead.addParam(DXL_ID);
  if (dxl_addparam_result != true)
  {
    Serial.print("[ID:"); Serial.print(DXL_ID); Serial.println("groupSyncRead addparam failed");
    return;
  }

  while(1)
  {
    Serial.print("Press any key to continue! (or press q to quit!)\n");
    if (getch() == 'q')
      break;

    // Allocate LED and goal position value into byte array
    param_indirect_data_for_write[0] = DXL_LOBYTE(DXL_LOWORD(dxl_goal_position[index]));
    param_indirect_data_for_write[1] = DXL_HIBYTE(DXL_LOWORD(dxl_goal_position[index]));
    param_indirect_data_for_write[2] = DXL_LOBYTE(DXL_HIWORD(dxl_goal_position[index]));
    param_indirect_data_for_write[3] = DXL_HIBYTE(DXL_HIWORD(dxl_goal_position[index]));
    param_indirect_data_for_write[4] = dxl_led_value[index];

    // Add values to the Syncwrite storage
    dxl_addparam_result = groupSyncWrite.addParam(DXL_ID, param_indirect_data_for_write);
    if (dxl_addparam_result != true)
    {
      Serial.print("[ID:"); Serial.print(DXL_ID); Serial.println("groupSyncWrite addparam failed");
      return;
    }

    // Syncwrite all
    dxl_comm_result = groupSyncWrite.txPacket();
    if (dxl_comm_result != COMM_SUCCESS) Serial.print(packetHandler->getTxRxResult(dxl_comm_result));

    // Clear syncwrite parameter storage
    groupSyncWrite.clearParam();

    do
    {
      // Syncread present position from indirectdata2
      dxl_comm_result = groupSyncRead.txRxPacket();
      if (dxl_comm_result != COMM_SUCCESS) Serial.print(packetHandler->getTxRxResult(dxl_comm_result));

      // Check if groupsyncread data of Dyanamixel is available
      dxl_getdata_result = groupSyncRead.isAvailable(DXL_ID, ADDR_PRO_INDIRECTDATA_FOR_READ, LEN_PRO_PRESENT_POSITION);
      if (dxl_getdata_result != true)
      {
        Serial.print("[ID:"); Serial.print(DXL_ID); Serial.println("groupSyncRead getdata failed");
        return;
      }

      // Check if groupsyncread data of Dyanamixel is available
      dxl_getdata_result = groupSyncRead.isAvailable(DXL_ID, ADDR_PRO_INDIRECTDATA_FOR_READ + LEN_PRO_PRESENT_POSITION, LEN_PRO_MOVING);
      if (dxl_getdata_result != true)
      {
        Serial.print("[ID:"); Serial.print(DXL_ID); Serial.println("groupSyncRead getdata failed");
        return;
      }

      // Get Dynamixel present position value
      dxl_present_position = groupSyncRead.getData(DXL_ID, ADDR_PRO_INDIRECTDATA_FOR_READ, LEN_PRO_PRESENT_POSITION);

      // Get Dynamixel moving status value
      dxl_moving = groupSyncRead.getData(DXL_ID, ADDR_PRO_INDIRECTDATA_FOR_READ + LEN_PRO_PRESENT_POSITION, LEN_PRO_MOVING);

      Serial.print("[ID:"); Serial.print(DXL_ID);
      Serial.print("] GoalPos:"); Serial.print(dxl_goal_position[index]);
      Serial.print("  PresPos:"); Serial.print(dxl_present_position);
      Serial.print("  IsMoving:"); Serial.print(dxl_moving);
      Serial.println(" ");

    }while(abs(dxl_goal_position[index] - dxl_present_position) > DXL_MOVING_STATUS_THRESHOLD);

    // Change goal position
    if (index == 0)
    {
      index = 1;
    }
    else
    {
      index = 0;
    }
  }

  // Disable Dynamixel Torque
  dxl_comm_result = packetHandler->write1ByteTxRx(portHandler, DXL_ID, ADDR_PRO_TORQUE_ENABLE, TORQUE_DISABLE, &dxl_error);
  if (dxl_comm_result != COMM_SUCCESS)
  {
    Serial.print(packetHandler->getTxRxResult(dxl_comm_result));
  }
  else if (dxl_error != 0)
  {
    Serial.print(packetHandler->getRxPacketError(dxl_error));
  }

  // Close port
  portHandler->closePort();

  return;
}

void loop()
{

}
ottensmeyer commented 3 years ago

Update - tried with an XC430-W150-T - has the same indirect address/data addresses as the XM and works without problems. No idea why the XL doesn't seem to work as expected.

ROBOTIS-Will commented 3 years ago

Hi @ottensmeyer Thank you for your inquiry and sorry about your incovenience. In the current XL330 firmware, there's a bug that some indirect address and data are not properly linked. The next firmware update includes the fix for this bug.

ottensmeyer commented 3 years ago

Thanks Will - glad it wasn't me doing something dumb. Do you have an ETA on the next update? Would I need to update the Wizard to obtain the firmware update, or will the Wizard recognize the presence of an update for the 330 automagically?

Cheers Mark

ROBOTIS-Will commented 3 years ago

Hi @ottensmeyer Thank you for your understanding. Once the Wizard 2.0 software is updated, you'll get a notification. The firmware update should be done manually as there are many customers who want to keep using a specific firmware version for their product. Wizard 2.0 supports multiple module firmware update and you don't need to remove any connection from the system for a simple firmware update. https://emanual.robotis.com/docs/en/software/dynamixel/dynamixel_wizard2/#firmware-update-all

The next update is scheduled in this month, but in case you urgently need to test the feature, please try the temporary firmware below. Please note that this firmware fixes the indirect address & data bug and provided as is which does not guarantee its integrity. https://drive.google.com/file/d/1a3gQKBToUtmg_l8UejzMILWscYeQwp7s/view?usp=sharing Download will be available for the next 7 days. Thank you!

ottensmeyer commented 3 years ago

Hi Will - just installed the temporary fix - looks like it's working fine - thanks very much for the early access - will keep my eyes out for the full update!

best, Mark

On Mon, May 31, 2021 at 8:22 PM Will Son @.***> wrote:

Hi @ottensmeyer https://github.com/ottensmeyer Thank you for your understanding. Once the Wizard 2.0 software is updated, you'll get a notification. The firmware update should be done manually as there are many customers who want to keep using a specific firmware version for their product. Wizard 2.0 supports multiple module firmware update and you don't need to remove any connection from the system for a simple firmware update.

https://emanual.robotis.com/docs/en/software/dynamixel/dynamixel_wizard2/#firmware-update-all

The next update is scheduled in this month, but in case you urgently need to test the feature, please try the temporary firmware below. Please note that this firmware fixes the indirect address & data bug and provided as is which does not guarantee its integrity.

https://drive.google.com/file/d/1a3gQKBToUtmg_l8UejzMILWscYeQwp7s/view?usp=sharing Download will be available for the next 7 days. Thank you!

— You are receiving this because you were mentioned. Reply to this email directly, view it on GitHub https://github.com/ROBOTIS-GIT/DynamixelSDK/issues/502#issuecomment-851726859, or unsubscribe https://github.com/notifications/unsubscribe-auth/ADEPGIF2QZGD4AZTLOXT5MTTQQR4FANCNFSM45XD7TGQ .