Reading Present Position Value too slow

[aeht]

I have an OpenCM9.04 + OpenCM485 EXP and using Dynamixel2Arduino library to talk to my MX-106T (2.0) servo. When i set the goal position and try to read present pos it's very slow/quite delayed.

bool success = dxl.setGoalPosition(hardwareId, pos); 
presentPos = dxl.getPresentPosition(hardwareId); 

For example, if I am trying to open from 2100 to 3200, the mx-106t will move and the pos you can see in real life is 3180 but the present pos being read is still ~2100 (previous position instead of present position). Even after doing a do-while loop like this:

 do{
    presentPos = dxl.getPresentPosition(hardwareId); 
  }while(abs(pos-presentPos)>20);
 

presentPos is delayed, if the servo moves slow enough. I changed moving speed to 60, and it cannot read position data properly in this speed. But at faster speeds like 150, it can keep up with the position a little bit better.

Additionally, Talking to servo is only succesful at 57600. Any baud rate greater than that, the mx-106t is unresponsive. Is there a reason why this happens? And how to fix?

[ROBOTIS-Will]

Hi @aeht

Continuously printing data on the terminal may lag depending on your environment especially if you are printing various information through the serial port.
However, in my case, the printed Present Position didn't have much discrepancy with the actual position.

In order to change the baudrate, you need to change the buadrate of DYNAMIXEL.
Please see the modified code below.

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

#include <Dynamixel2Arduino.h>

// Please modify it to suit your hardware.
#if defined(ARDUINO_AVR_UNO) || defined(ARDUINO_AVR_MEGA2560) // When using DynamixelShield
  #include <SoftwareSerial.h>
  SoftwareSerial soft_serial(7, 8); // DYNAMIXELShield UART RX/TX
  #define DXL_SERIAL   Serial
  #define DEBUG_SERIAL soft_serial
  const uint8_t DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN
#elif defined(ARDUINO_SAM_DUE) // When using DynamixelShield
  #define DXL_SERIAL   Serial
  #define DEBUG_SERIAL SerialUSB
  const uint8_t DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN
#elif defined(ARDUINO_SAM_ZERO) // When using DynamixelShield
  #define DXL_SERIAL   Serial1
  #define DEBUG_SERIAL SerialUSB
  const uint8_t DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN
#elif defined(ARDUINO_OpenCM904) // When using official ROBOTIS board with DXL circuit.
  #define DXL_SERIAL   Serial3 //OpenCM9.04 EXP Board's DXL port Serial. (Serial1 for the DXL port on the OpenCM 9.04 board)
  #define DEBUG_SERIAL Serial
  const uint8_t DXL_DIR_PIN = 22; //OpenCM9.04 EXP Board's DIR PIN. (28 for the DXL port on the OpenCM 9.04 board)
#elif defined(ARDUINO_OpenCR) // When using official ROBOTIS board with DXL circuit.
  // For OpenCR, there is a DXL Power Enable pin, so you must initialize and control it.
  // Reference link : https://github.com/ROBOTIS-GIT/OpenCR/blob/master/arduino/opencr_arduino/opencr/libraries/DynamixelSDK/src/dynamixel_sdk/port_handler_arduino.cpp#L78
  #define DXL_SERIAL   Serial3
  #define DEBUG_SERIAL Serial
  const uint8_t DXL_DIR_PIN = 84; // OpenCR Board's DIR PIN.    
#else // Other boards when using DynamixelShield
  #define DXL_SERIAL   Serial1
  #define DEBUG_SERIAL Serial
  const uint8_t DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN
#endif
 

const uint8_t DXL_ID = 1;
const float DXL_PROTOCOL_VERSION = 2.0;

Dynamixel2Arduino dxl(DXL_SERIAL, DXL_DIR_PIN);

//This namespace is required to use Control table item names
using namespace ControlTableItem;

void setup() {
  // put your setup code here, to run once:
  
  // Use UART port of DYNAMIXEL Shield to debug.
  DEBUG_SERIAL.begin(57600);
  while(!DEBUG_SERIAL);

  // Set Port baudrate to 57600bps. This has to match with DYNAMIXEL baudrate.
  dxl.begin(57600);
  // Set Port Protocol Version. This has to match with DYNAMIXEL protocol version.
  dxl.setPortProtocolVersion(DXL_PROTOCOL_VERSION);
  // Get DYNAMIXEL information
  dxl.ping(DXL_ID);

  // Change Baudrate from 57600 to 1000000
  dxl.setBaudrate(DXL_ID, 1000000);
  dxl.begin(1000000);

  // Turn off torque when configuring items in EEPROM area
  dxl.torqueOff(DXL_ID);
  dxl.setOperatingMode(DXL_ID, OP_POSITION);
  dxl.torqueOn(DXL_ID);

  // Limit the maximum velocity in Position Control Mode. Use 0 for Max speed
  dxl.writeControlTableItem(PROFILE_VELOCITY, DXL_ID, 30);
}

void loop() {
  // put your main code here, to run repeatedly:
  
  // Please refer to e-Manual(http://emanual.robotis.com/docs/en/parts/interface/dynamixel_shield/) for available range of value. 
  // Set Goal Position in RAW value
  dxl.setGoalPosition(DXL_ID, 4095);

  int i_present_position = 0;
  float f_present_position = 0.0;

  while (abs(4095 - i_present_position) > 10)
  {
    f_present_position = dxl.getPresentPosition(DXL_ID, UNIT_DEGREE);
    i_present_position = dxl.getPresentPosition(DXL_ID);

    DEBUG_SERIAL.println(i_present_position);
  }
  delay(500);

  // Set Goal Position in DEGREE value
  dxl.setGoalPosition(DXL_ID, 5.7, UNIT_DEGREE);
  
  while (abs(5.7 - f_present_position) > 2.0)
  {
    f_present_position = dxl.getPresentPosition(DXL_ID, UNIT_DEGREE);
    i_present_position = dxl.getPresentPosition(DXL_ID);

    DEBUG_SERIAL.println(i_present_position);
  }
  delay(500);
}

[aeht]

@ROBOTIS-Will Thanks Will! Sorry I never got back to this. I eventually figured out what was going on with the reading issue and now can receive feedback from the servos just fine. However, baud rate is still a problem for me. I followed what you have above to change baud rate, but for some reason it doesn't affect the dynamixel.

I do the ff:

dxl.begin(57600); 
dxl.torqueOff(DXL_ID);
dxl.setBaudRate(DXL_ID, 1000000);
dxl.begin(1000000);
dxl.torqueOn(DXL_ID);

I also tried

dxl.begin(57600); 
dxl.torqueOff(DXL_ID);
dxl.setBaudRate(DXL_ID, 1000000);
dxl.torqueOn(DXL_ID);
dxl.begin(1000000);

and

dxl.begin(57600); 
dxl.torqueOff(DXL_ID);
dxl.write(DXL_ID, 4, (uint8_t*)1, 1, 10); // 1 because of what I found 1=1Mbps https://emanual.robotis.com/docs/en/dxl/mx/mx-106/
dxl.begin(1000000);
dxl.torqueON(DXL_ID);

But no luck. For some reason there is still mismatch with the DXL baud rate. I can however change the baud rates via the Dynamixel wizard 2.0 software. But it would be better if I can do it programmatically. I realized too that I am working with MX-106Ts protocol 1.0. Does that affect anything?