...
POST No. 2407000
Reading Position and Load from three dynamixel MX-106 at a time
2018-10-11 14:35:10 rsbisht

Note: Previous Reference- 

http://en.robotis.com/service/forum_view.php?slg=&page_type=&bbs_no=2406990 



I am using 3 Mx-106 in series. Is that possible to read Position and Load at the same time of these three motors?


I am trying by flowing code:


  present_position_ID1 = dxl_wb.itemRead(DXL_ID1, "Present_Position");

  present_speed_ID1 = dxl_wb.itemRead(DXL_ID1, "Present_Speed");

  Present_load_ID1 = dxl_wb.itemRead(DXL_ID1, "Present_Current");

  Serial.println("present_position_ID1");

  Serial.println(present_position_ID1);

  Serial.println("Present_load_ID1");

  Serial.println(Present_load_ID1);


  //ID2 Read

present_position_ID2 = dxl_wb.itemRead(DXL_ID2, "Present_Position");

Present_load_ID2 = dxl_wb.itemRead(DXL_ID2, "Present_Current");

Serial.println("present_position_ID2");

Serial.println(present_position_ID2);

Serial.println("Present_load_ID2");

Serial.println(Present_load_ID2);


.......


Please let me know what else I can do.

I am also using BT-210 to control the Dynamixel by R+m. Task2.

Dynamixel stop working when above code is used otherwise they works properly.

We are using Dynamxel Workbench and Arduino IDE with Protocol-2.


Thank you 


2018-10-11 14:35:10
ravindra singh bisht
2018-10-12 09:12:50 Kurt

Normal caveat :D


Again I don't use the Workbench.  But I would suggest looking in the e-manual: http://emanual.robotis.com/docs/en/software/dynamixel/dynamixel_workbench/ at either Sync Read or Bulk Read.

There are a couple of examples installed by the Arduino installs for OpenCM9.04.  So look at the examples under OpenCM...->Dynamixel Wench

for both q_Sync_Read_Write and r_Bulk_Read_Write.


Note I am not sure with the Workbench if you can do this all in one or not...


With the Dynamixel SDK library, I would note that the these three values can be read starting at address 126 with 10 bytes http://emanual.robotis.com/docs/en/dxl/mx/mx-106-2/#present-current

and I would setup a Protocol 2 Sync Read: http://emanual.robotis.com/docs/en/dxl/protocol2/#sync-read


Not sure if there is a good example with OpenCM... Maybe I missed it.  I believe there is a limited on for OPenCR, which you can get if you have OPenCR Arduino setup on your machine.  Or the example can be found up at: https://github.com/ROBOTIS-GIT/OpenCR/blob/master/arduino/opencr_arduino/opencr/libraries/OpenCR/examples/07.%20DynamixelSDK/protocol2.0/sync_read_write/sync_read_write.ino


With the example they just read in one field.  But you can instead read in 10 bytes per ID and then extract the three fields...


Good luck


And again Will Son will probably more complete instructions



2018-10-12 09:12:50
Kurt
2018-10-12 21:21:17 ravindra singh bisht

thank you @ Kurt and waiting reply from Will Son..


regards 

R S Bisht

2018-10-12 21:21:17
ravindra singh bisht
2018-10-12 23:57:02 Kurt

You are welcome,

Have you looked at some of the examples above?  Again with the Workbench code I believe you are pretty limited.  My guess is the best chance is with the SyncRead as the bulk read allows you to read different addresses for each servo, but only one item per servo...    


Example hacked up version of the Sync read.  It compiles, not sure it would work for you or not...

/*******************************************************************************
  Copyright 2016 ROBOTIS CO., LTD.

  Licensed under the Apache License, Version 2.0 (the "License");
  you may not use this file except in compliance with the License.
  You may obtain a copy of the License at

      http://www.apache.org/licenses/LICENSE-2.0

  Unless required by applicable law or agreed to in writing, software
  distributed under the License is distributed on an "AS IS" BASIS,
  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  See the License for the specific language governing permissions and
  limitations under the License.
*******************************************************************************/

/* Authors: Taehun Lim (Darby) */

#include

#if defined(__OPENCM904__)
#define DEVICE_NAME "3" //Dynamixel on Serial3(USART3)  <-OpenCM 485EXP
#elif defined(__OPENCR__)
#define DEVICE_NAME ""
#endif

#define BAUDRATE  57600
#define DXL_ID_1  1
#define DXL_ID_2  2
#define DXL_ID_3  3

DynamixelWorkbench dxl_wb;

const uint8_t dxl_cnt = 3;
uint8_t dxl_id[dxl_cnt] = {DXL_ID_1, DXL_ID_2, DXL_ID_3};

int32_t goal_position[2] = {1000, 3000};

int32_t present_position[dxl_cnt] = {0, 0};
int32_t present_velocity[dxl_cnt] = {0, 0};
int32_t present_current [dxl_cnt] = {0, 0};

void setup()
{
  Serial.begin(57600);
  // while(!Serial); // If this line is activated, you need to open Serial Terminal.

  dxl_wb.begin(DEVICE_NAME, BAUDRATE);

  for (int cnt = 0; cnt < dxl_cnt; cnt++)
  {
    dxl_wb.ping(dxl_id[cnt]);
    dxl_wb.jointMode(dxl_id[cnt]);
  }

  dxl_wb.addSyncWrite("Goal_Position");
  dxl_wb.addSyncRead("Present_Current");
  dxl_wb.addSyncRead("Present_Position");
  dxl_wb.addSyncRead("Present_Velocity");
}

void loop()
{
  dxl_wb.syncWrite("Goal_Position", goal_position);

  do
  {
    int32_t *get_data;

    get_data = dxl_wb.syncRead("Present_Velocity");
    for (int cnt = 0; cnt < dxl_cnt; cnt++)
      present_velocity[cnt] = get_data[cnt];

    get_data = dxl_wb.syncRead("Present_Position");
    for (int cnt = 0; cnt < dxl_cnt; cnt++)
      present_position[cnt] = get_data[cnt];

    get_data = dxl_wb.syncRead("Present_Current");
    for (int cnt = 0; cnt < dxl_cnt; cnt++)
      present_current[cnt] = get_data[cnt];

    log();
  } while (abs(goal_position[0] - present_position[0]) > 20 &&
           abs(goal_position[1] - present_position[1]) > 20);

  swap();
}

void log()
{
  for (int cnt = 0; cnt < dxl_cnt; cnt++)
  {
    Serial.print("[ ID : "    + String(dxl_id[cnt])           +
                 " GoalPos: " + String(goal_position[cnt])    +
                 " PresPos: " + String(present_position[cnt]) +
                 " PresVel: " + String(present_velocity[cnt]) +
                 " PresCur: " + String(present_current[cnt])  +
                 " ]  ");
  }
  Serial.println("");
}

void swap()
{
  int32_t tmp = goal_position[0];
  goal_position[0] = goal_position[1];
  goal_position[1] = tmp;
}

But understand this might help a little here, but still does not reduce the amount of DXL buss or USB buss transfers by that much.   That is currently you are doing 3 itemReads per servo where each of these reads does a TX packet followed by RX packet.  So you have 9 TX packets and 9 RX packets for the loops.


The example I did above does one TX packet for each address you are wanting, and each servo returns a packet for each of these calls, so: you have 3 TX packets and still 9 RX packets.

So again it helps a little. 


Note: there are lots of things here I don't like, including: no control over what servos you wish to do the query for.  It appears to use magic to decide.  That is it remembers all of the servos you talked to, which is done by the Ping loop...


Could do something similar with the Bulk Read write example, with some more complicated setup, like maybe 3 of the GroupBulkRead objects, each one init to read a specific parameter for each servo... And then process it...  But again this would get you to 3Tx and 9Rx...  


Actually looking at how this one is done.  The Bulk Read code in Dynamixel Workbench is to me goes against the grain of this library as it does not use Symbolic names for what address to start reading from nor know the type of servo it is to know the size of the field to read.  So looks like it may be a straight pass through to Dynamixel SDK...

EDIT: oops looked at Dynamixel SDK example  So I don't see way with the Workbench to tell the BulkRead to read in the three fields, even though in SDK no problem...



EDIT: With SDK Bulk read I think you can do something like:


#define LENGTH_CURRENT_THROUGH_POSITION (2+4+4)

 dynamixel::GroupBulkRead groupBulkRead(portHandler, packetHandler);

dxl_addparam_result = groupBulkRead.addParam(DXL_ID_1, ADDRESS_PRESENT_CURRENT , LENGTH_CURRENT_THROUGH_POSITION );

dxl_addparam_result = groupBulkRead.addParam(DXL_ID_2, ADDRESS_PRESENT_CURRENT , LENGTH_CURRENT_THROUGH_POSITION );

dxl_addparam_result = groupBulkRead.addParam(DXL_ID_3, ADDRESS_PRESENT_CURRENT , LENGTH_CURRENT_THROUGH_POSITION );

...

dxl_comm_result = groupBulkRead.txRxPacket();

...

// Check if groupbulkread data of Dynamixel#1 is available
  dxl_getdata_result = groupBulkRead.isAvailable(DXL_ID_1, ADDRESS_PRESENT_CURRENT , LENGTH_CURRENT_THROUGH_POSITION )

...

/// ****************************************** BUT ************************************

for (int cnt = 0; cnt < dxl_cnt; cnt++)

    if (groupBulkRead.isAvailable(dxl_id[cnt], 126,10 ) == COMM_SUCCESS) {

        present_current[cnt] = groupBulkRead.getData(dxl_id[cnt], 126, 2);

        present_velocityt[cnt] = groupBulkRead.getData(dxl_id[cnt], 128, 4);

        present_velocityt[cnt] = groupBulkRead.getData(dxl_id[cnt], 132, 4);

    }

}

 

I would also create #define and use them so You dont see hard coded number like 126, 128 and 132...  


But again I don't see any way to do this with the Workbench as they only allow you to add by name one field AND you can not add the same ID twice...


Again Hopefully @Will Son will give you some additional hints,  but maybe this should give you enough to play with until the end of the weekend, which I believe has already started there...



2018-10-12 23:57:02
Kurt
2018-10-15 11:38:30 Will Son

Hi Ravindra,


Sorry for the late reply due to Robotworld 2018 last week.

I simply modified OpenCM9.04 q_Sync_Read_Write example in Dynamixel Workbench.


/*******************************************************************************
* Copyright 2016 ROBOTIS CO., LTD.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*******************************************************************************/

/* Authors: Taehun Lim (Darby) */

#include <DynamixelWorkbench.h>

#if defined(__OPENCM904__)
#define DEVICE_NAME "3" //Dynamixel on Serial3(USART3) <-OpenCM 485EXP
#elif defined(__OPENCR__)
#define DEVICE_NAME ""
#endif

#define BAUDRATE 57600
#define DXL_ID_1 1
#define DXL_ID_2 2

DynamixelWorkbench dxl_wb;

uint8_t dxl_id[2] = {DXL_ID_1, DXL_ID_2};
uint8_t dxl_cnt = 2;

int32_t goal_position[2] = {1000, 3000};

int32_t present_position[2] = {0, 0};
int32_t present_velocity[2] = {0, 0};
int16_t present_current[2] = {0, 0};

void setup()
{
Serial.begin(57600);
while(!Serial); // If this line is activated, you need to open Serial Terminal.

dxl_wb.begin(DEVICE_NAME, BAUDRATE);

for (int cnt = 0; cnt < dxl_cnt; cnt++)
{
dxl_wb.ping(dxl_id[cnt]);
dxl_wb.jointMode(dxl_id[cnt]);
}

dxl_wb.addSyncWrite("Goal_Position");
dxl_wb.addSyncRead("Present_Current");
dxl_wb.addSyncRead("Present_Position");
dxl_wb.addSyncRead("Present_Velocity");
}

void loop()
{
dxl_wb.syncWrite("Goal_Position", goal_position);

do
{
int32_t *get_data;

get_data = dxl_wb.syncRead("Present_Velocity");
for (int cnt = 0; cnt < dxl_cnt; cnt++)
present_velocity[cnt] = get_data[cnt];

get_data = dxl_wb.syncRead("Present_Position");
for (int cnt = 0; cnt < dxl_cnt; cnt++)
present_position[cnt] = get_data[cnt];

get_data = dxl_wb.syncRead("Present_Current");
for (int cnt = 0; cnt < dxl_cnt; cnt++)
present_current[cnt] = get_data[cnt];

log();
}while(abs(goal_position[0] - present_position[0]) > 20 &&
abs(goal_position[1] - present_position[1]) > 20);

swap();
}

void log()
{
for (int cnt = 0; cnt < dxl_cnt; cnt++)
{
Serial.print("[ ID : " + String(dxl_id[cnt]) +
" GoalPos: " + String(goal_position[cnt]) +
" PresCur: " + String(present_current[cnt]) +
" PresPos: " + String(present_position[cnt]) +
" PresVel: " + String(present_velocity[cnt]) +
" ] ");
}
Serial.println("");
}

void swap()
{
int32_t tmp = goal_position[0];
goal_position[0] = goal_position[1];
goal_position[1] = tmp;
}



 

Sync read is only available when target registers are consecutive. If you are trying to read from separate addresses(non-consecutive), you should use bulk read or apply indirect addresses and data to make them consecutive.

Hope this example helps you.

Thanks!

comment
2018-10-15 22:50:07 Kurt
@Will Son - I think that is more or less the same as the example code I put in my second response to this thread.

Unfortunately I believe that each call to addSyncRead creates a new dynamixel::GroupSyncRead object, which when you call to syncRead, it will call through and generate a set of messages (one tx message and hopefully 3 RX messages...), so again it saves a little but not as nice if you instead could use Dynamixel SDK, where you could specify that you wish to read enough bytes from the servo to handle more than one address...

Maybe I am wrong, but that is my reading of the code...
2018-10-15 22:50:07
Kurt
2018-10-16 15:40:44 Will Son
Kurt,

Yes, you're right. The current DynamixelWorkbench does not support reading multi items using SyncRead as it only accepts item name to read as a parameter and acquire address and length.

Darby is planning to update DynamixelWorkbench, but SDK definitely opens up the full feature of Dynamixel.

Thank you for pointing out an important issue! :D
2018-10-16 15:40:44
Will Son
2018-10-15 11:38:30
Will Son
Reply
웹에디터 시작 웹 에디터 끝