Open-Manipulato-X Current Control with Dyna-Workbench, reading issue values high

[MAFlanagan]

ISSUE TEMPLATE ver. 1.0.0

Before you open issue, please refer to ROBOTIS e-Manual

1. How to setup? (ex, U2D2, OpenCR,...)
   OpenCR

2. Which Dynamixel have you used? and how many? (Please describe below format to all connected Dynamixels)

   • Model Name
     XM430-W350-T

   • ID
     5 Motors, ID num 11-15

   • Baud Rate of Dynamixels
     1MB

   • Protocol Version
     2

3. Write down the commands you used in order

I'm using the turtlebot3 with open manipulator firmware found in the OpenCR

(OpenCR/arduino/opencr_arduino/opencr/libraries/turtlebot3/examples/turtlebot3_with_open_manipulator/turtlebot3_with_open_manipulator_core/)

I added the sync write and read handlers for goal current

result = dxl_wb_.addSyncWriteHandler(ADDR_GOAL_CURRENT, LEN_GOAL_CURRENT, &log);
if (result == false)
{
  DEBUG_SERIAL.println(log);
  DEBUG_SERIAL.println("Failed to add sync write handler");
}
result = dxl_wb_.addSyncReadHandler(ADDR_GOAL_CURRENT, 
                                    LEN_GOAL_CURRENT, 
                                    &log);
if (result == false)
{
  DEBUG_SERIAL.println(log);
  DEBUG_SERIAL.println("Failed to add sync write handler\n");
  return false;
}

Then added the function to switch to current control mode

bool OpenManipulatorDriver::modesetCurrent()
{
const char *log;
uint16_t model_number = 0;
for (int cnt = 1; cnt < 2; cnt++)
{
bool result = dxl_wb_.ping(joint_.id[cnt], &model_number, &log);
if (result == false)
{
DEBUG_SERIAL.println(log);
DEBUG_SERIAL.println("Failed to ping\n");
return false;
}
else
{
DEBUG_SERIAL.println("Succeeded to ping\n");
}
result = dxl_wb_.torque(joint_.id[cnt], 0);
if (result == false)
{
DEBUG_SERIAL.println(log);
DEBUG_SERIAL.println("Failed to change torque enabled \n");
return false;
}
else
{
DEBUG_SERIAL.println("Succeed to change torque enabled\n");
}
result = dxl_wb_.setCurrentControlMode(joint_.id[cnt], &log);
if (result == false)
{
DEBUG_SERIAL.println(log);
DEBUG_SERIAL.println("Failed to change joint mode\n");
return false;
}
else
{
DEBUG_SERIAL.println("Succeed to change joint mode\n");
}
result = dxl_wb_.torque(joint_.id[cnt], 1);
if (result == false)
{
DEBUG_SERIAL.println(log);
DEBUG_SERIAL.println("Failed to change torque enabled \n");
return false;
}
else
{
DEBUG_SERIAL.println("Succeed to change torque enabled\n");
}
}
return true;
}

The jointcontrol function was commented out.

Then I read and write to the Goal Current address

5. Please, describe detailedly what difficulty you are in

When in Current Control Mode, present position/velocity/current values are all really high and dont change. The instant I switch modes they go to high values and dont change regardless of the motors position etc.

The values im reading from the goal current address (which is what im writing to) are correct its just the present values which are appearing high. When in dynamixel wizard 2.0 the values are correct.

I've tested it in position mode and it works fine. its only when i switch modes.

[MAFlanagan]

Fixed it.

I changed this function

result = dxl_wb_.setCurrentControlMode(joint_.id[cnt], &log);

to

result = dxl_wb_.setOperatingMode(joint_.id[cnt], 0, &log);

and it worked which is strange as the setCurrentControlMode calls setOperatingMode

[ROBOTIS-Will]

Hi @MAFlanagan

Thanks for the report and update.
Right, the setCurrentControlMode should do the same as setOperatingMode as below.

bool DynamixelWorkbench::setCurrentControlMode(uint8_t id, const char **log)
{
  bool result = false;

  result = setOperatingMode(id, CURRENT_CONTROL_MODE, log);

  if (result == false)
  {
    if (log != NULL) *log = "[DynamixelWorkbench] Failed to set Current Control Mode!";
    return false;
  }

  if (log != NULL) *log = "[DynamixelWorkbench] Succeeded to set Current Control Mode!";
  return result;
}

What's even weird for me is that the present values should always available regardless of the operating mode as they will be kept updated when the DYNAMIXEL is running.

[MAFlanagan]

Hi @ROBOTIS-Will

Yeah this is what confused me as I tried using some of the other functions which set the operating mode (which call setOperatingMode in them) and they all had the same result. I just tried setoperatingmode on a whim and it works. no idea why but it solved about 2 days worth of fault finding so im not going to complain.

as for the not reading the values, again ive got no idea why. think i read the emanual through about three times looking for some reason why it would do this or something I missed and couldnt find anything. Seems to be one of those weird instances where code just does what it wants, regardless of reason.