Interestingly, serial.print commands conflict

[parworker]

I have a code as follows. If I remove the commands I set as comments in the checkbaudrate function in the checkbaudrate.ino file in this code, the location information I send for the positioning of the servo is not processed correctly on the code side, and this causes the servon to not move to the desired position. In other words, the servos do not move at all.
Does anyone know the reason for the problem here?
Note: The UNITconversion.ino and Checkbaudrate.ino files I shared are used as tabs in the main code.

Dynamixelmain.ino

#include <Dynamixel2Arduino.h>  //The library has never been modified
/* ---------------------------------------CONFIGURATION_PARAMETERS--------------------------------------------------------*/
const float DYNAMIXEL_PROTOCOL_VERSION = 2.0;  //Don't change this... !
const uint8_t BROADCAST_ID = 254;              //
const uint8_t SERVO_1_ID = 1;                  //previously set
const uint8_t SERVO_2_ID = 2;                  //previously set
// const uint8_t SERVO_3_ID = 3;
// const uint8_t SERVO_4_ID = 4;
const uint8_t DXL_ID_CNT = 2;  //
const uint8_t DXL_ID_LIST[DXL_ID_CNT] = { SERVO_1_ID, SERVO_2_ID, /*SERVO_3_ID,SERVO_4_ID*/ };
uint32_t DESIRED_BAUDRATE = 57600;
//----------------------------------------------------------------------------------------------------------------------------
int32_t CWAX_POSITION_LIMIT = 360;  //CW Degree
int32_t CCWAX_POSITION_LIMIT = 0;   //CCW Degree
uint16_t position_p_gain = 850;
uint16_t position_i_gain = 0;
uint16_t position_d_gain = 0;
int32_t profile_velocity = 50.0;    //RPM()
int32_t profile_acceleration = 50;  // RPM square^2()
/*-------------------------------------------------------^_^------------------------------------------------------------------*/
//-------------------------------------------introducing the board to be used--------------------------------------------------
#if defined(ARDUINO_OpenCR)
#define DXL_SERIAL Serial3
#define DEBUG_SERIAL Serial
const int DXL_DIR_PIN = 84;
#endif
Dynamixel2Arduino dxl(DXL_SERIAL, DXL_DIR_PIN);
using namespace ControlTableItem;
//------------------------------------------------CONFIGURATION_OF_SYNC_WRITE-READ-----------------------------------------------
const uint16_t user_pkt_buf_cap = 128;
uint8_t user_pkt_buf[user_pkt_buf_cap];
const uint16_t SR_START_ADDR = 126;
const uint16_t SR_ADDR_LEN = 12;
const uint16_t SW_START_ADDR = 116;
const uint16_t SW_ADDR_LEN = 4;
//----------------------READ_DATA_TYPE_STRUCTURE-----------------------------------------------------------------------
typedef struct sr_data {
  int16_t present_current;
  int32_t present_velocity;
  int32_t present_position;
} __attribute__((packed)) sr_data_t;
//----------------------WRITE_DATA_TYPE_STRUCTURE-----------------------------------------------------------------------
typedef struct sw_data {
  int32_t goal_position;
} __attribute__((packed)) sw_data_t;
//----------------------WRITE_AND_READ_İNFOS-----------------------------------------------------------------------------
sr_data_t sr_data[DXL_ID_CNT];
DYNAMIXEL::InfoSyncReadInst_t sr_infos;
DYNAMIXEL::XELInfoSyncRead_t info_xels_sr[DXL_ID_CNT];

sw_data_t sw_data[DXL_ID_CNT];
DYNAMIXEL::InfoSyncWriteInst_t sw_infos;
DYNAMIXEL::XELInfoSyncWrite_t info_xels_sw[DXL_ID_CNT];

//-----------------------------------------SERIAL_READ_BUFFER_VERIABLES-------------------------------------------------

const uint8_t BUFFER_SIZE = 64;
char inputBuffer[BUFFER_SIZE];
uint8_t bufferIndex = 0;
/*------------------------------------------------------------------------------------------------------------------------*/
void setup() {
  //---------------------------------SERIAL_AND_DYNAMİXEL_PROTOCOL_CONFIGURATIONS--------------------------------------------
  DEBUG_SERIAL.begin(57600);
  dxl.begin(DESIRED_BAUDRATE);
  dxl.setPortProtocolVersion(DYNAMIXEL_PROTOCOL_VERSION);
  // //--------------------------------------------------------DYNAMİXEL_SERVO_SETTINGS--------------------------------------------------------------
  for (int i = 0; i < DXL_ID_CNT; i++) {  // Scan all dynamixel
    if (dxl.ping(DXL_ID_LIST[i]) == false) {  // if return not true
      check_baudrate(DXL_ID_LIST[i], DESIRED_BAUDRATE);
      // set baudrate
    } else {
      DEBUG_SERIAL.print("Already correct Baudrate: ");
      DEBUG_SERIAL.println(DXL_ID_LIST[i]);
    }
  }
  uint8_t i;                          //Count of dynamixels variable
  for (i = 0; i < DXL_ID_CNT; i++) {  // Write conf. for all servos
    dxl.torqueOff(DXL_ID_LIST[i]);
    dxl.writeControlTableItem(MAX_POSITION_LIMIT, DXL_ID_LIST[i], convertUnit2Value(CWAX_POSITION_LIMIT, "UNIT_DEGREE"));
    dxl.writeControlTableItem(MIN_POSITION_LIMIT, DXL_ID_LIST[i], convertUnit2Value(CCWAX_POSITION_LIMIT, "UNIT_DEGREE"));
    dxl.setOperatingMode(DXL_ID_LIST[i], OP_POSITION);
    dxl.writeControlTableItem(POSITION_P_GAIN, DXL_ID_LIST[i], position_p_gain);
    dxl.writeControlTableItem(POSITION_I_GAIN, DXL_ID_LIST[i], position_i_gain);
    dxl.writeControlTableItem(POSITION_D_GAIN, DXL_ID_LIST[i], position_d_gain);
    dxl.writeControlTableItem(PROFILE_ACCELERATION, DXL_ID_LIST[i], convertUnit2Value(profile_acceleration, "UNIT_ACCELERATION"));
    dxl.writeControlTableItem(PROFILE_VELOCITY, DXL_ID_LIST[i], convertUnit2Value(profile_velocity, "UNIT_RPM"));
  }
  //------------------------------------------------INITIALIZE_READING_PACKET----------------------------------------------------------------------
  dxl.torqueOn(BROADCAST_ID);
  sr_infos.packet.p_buf = user_pkt_buf;
  sr_infos.packet.buf_capacity = user_pkt_buf_cap;
  sr_infos.packet.is_completed = false;
  sr_infos.addr = SR_START_ADDR;
  sr_infos.addr_length = SR_ADDR_LEN;
  sr_infos.p_xels = info_xels_sr;
  sr_infos.xel_count = 0;

  for (i = 0; i < DXL_ID_CNT; i++) {
    info_xels_sr[i].id = DXL_ID_LIST[i];
    info_xels_sr[i].p_recv_buf = (uint8_t*)&sr_data[i];  //Pointer for  info_xels_sr Array to sr_data array for p_recv_buf
    sr_infos.xel_count++;
  }
  sr_infos.is_info_changed = true;
  //------------------------------------------------INITIALIZE_WRITING_PACKET----------------------------------------------------------------------
  sw_infos.packet.p_buf = nullptr;
  sw_infos.packet.is_completed = false;
  sw_infos.addr = SW_START_ADDR;
  sw_infos.addr_length = SW_ADDR_LEN;
  sw_infos.p_xels = info_xels_sw;
  sw_infos.xel_count = 0;

  for (i = 0; i < DXL_ID_CNT; i++) {
    info_xels_sw[i].id = DXL_ID_LIST[i];
    info_xels_sw[i].p_data = (uint8_t*)&sw_data[i].goal_position;
    sw_infos.xel_count++;
  }
  //--------------------------------------------------SERVO_INITIALIZE_POSITION-------------------------------------
  sw_data[0].goal_position = 2047;  // 180 Degree
  sw_data[1].goal_position = 2047;  //180 Degree
  sw_infos.is_info_changed = true;
  if (dxl.syncWrite(&sw_infos) == true) {
    DEBUG_SERIAL.println("[SyncWrite] Success");
  } else {
    DEBUG_SERIAL.print("[SyncWrite] Fail, Lib error code: ");
    DEBUG_SERIAL.print(dxl.getLastLibErrCode());
  }
}
/*------------------------------------------------------------------------------------------------------------------------*/
void loop() {
  //------------------------------------------------>READ_SERIAL_MONITOR<-----------------------------------------------------
  uint8_t i, recv_cnt;

  if (DEBUG_SERIAL.available() > 0) {  // !!!!!!! Select New line....

    char incomingChar = DEBUG_SERIAL.read();
    if (incomingChar == '\n' || bufferIndex >= BUFFER_SIZE - 1) {
      inputBuffer[bufferIndex] = '\0';
      DEBUG_SERIAL.println(inputBuffer);
      delay(500);
      processInput(inputBuffer);
      bufferIndex = 0;
    } else {
      inputBuffer[bufferIndex] = incomingChar;
      bufferIndex++;
    }
  }
  //--------------------------------------SERIAL_PRINT_DATA_FROM_SERVOS---------------------------------------------------------
  recv_cnt = dxl.syncRead(&sr_infos);
  if (recv_cnt > 0) {
    DEBUG_SERIAL.print("[SyncRead] Success, Received ID Count: ");
    DEBUG_SERIAL.println(recv_cnt);
    for (i = 0; i < recv_cnt; i++) {  //Read for all servos
      DEBUG_SERIAL.print("ID:");
      DEBUG_SERIAL.print(sr_infos.p_xels[i].id);
      DEBUG_SERIAL.print(" Error: ");
      DEBUG_SERIAL.print(sr_infos.p_xels[i].error);
      DEBUG_SERIAL.print(" Goal Position: ");
      DEBUG_SERIAL.print(sw_data[i].goal_position);
      DEBUG_SERIAL.print(" Present_Current: ");
      DEBUG_SERIAL.print(convertValue2Unit(sr_data[i].present_current, "UNIT_MILLI_AMPERE"));
      DEBUG_SERIAL.print(" Present_Velocity: ");
      DEBUG_SERIAL.print(convertValue2Unit(sr_data[i].present_velocity, "UNIT_RPM"));
      DEBUG_SERIAL.print(" Present_Position: ");
      DEBUG_SERIAL.print(convertValue2Unit(sr_data[i].present_position, "UNIT_DEGREE"));
      DEBUG_SERIAL.println();
      delay(10);
    }
  } else {
    DEBUG_SERIAL.print("[SyncRead] Fail, Lib error code: ");
  }
  DEBUG_SERIAL.println("=======================================================");
}
/*-------------------------------------PROCCESING_READ_COMMANDS_FROM_SERIAL_MONITOR------------------------------------------*/
void processInput(const char* input) {

  char* commaPosition = strchr(input, ',');
  if (commaPosition) {
    *commaPosition = '\0';
    int servo1Value = atoi(input);
    int servo2Value = atoi(commaPosition + 1);
    sw_data[0].goal_position = servo1Value;
    sw_data[1].goal_position = servo2Value;
    sw_infos.is_info_changed = true;
    if (dxl.syncWrite(&sw_infos) == true) {
      DEBUG_SERIAL.println("[SyncWrite] Success");
    } else {
      DEBUG_SERIAL.print("[SyncWrite] Fail, Lib error code: ");
      DEBUG_SERIAL.print(dxl.getLastLibErrCode());
    }
    // DEBUG_SERIAL.println();
    // bufferIndex = 0;
  }
}

------------------UNITconversion.ino------

//-------------MX-106_2--UNİT_SCALE-----------------------------//
const float PERCENT_UNIT_SCALE = 0.024414;/**/
const float RPM_UNIT_SCALE = 0.229;
const float DEGREE_UNIT_SCALE = 0.087891;
const float MILLI_AMPERE_UNIT_SCALE = 3.36;
const float ACCELERATION_UNIT_SCALE = 214.58;
//-------------------------------------------------------------//
float convertValue2Unit(int value, String unit) {
  if (unit == "UNIT_PERCENT") {
    return value * PERCENT_UNIT_SCALE;
  } else if (unit == "UNIT_RPM") {
    return value * RPM_UNIT_SCALE;
  } else if (unit == "UNIT_DEGREE") {
    return value * DEGREE_UNIT_SCALE;
  } else if (unit == "UNIT_MILLI_AMPERE") {
    return value * MILLI_AMPERE_UNIT_SCALE;
  } else if (unit == "UNIT_ACCELERATION") {
    return value * MILLI_AMPERE_UNIT_SCALE;
  } else {

    return 0.0;  
  }
}

float convertUnit2Value(float value, String unit) {
  if (unit == "UNIT_PERCENT") {
    return value / PERCENT_UNIT_SCALE;
  } else if (unit == "UNIT_RPM") {
    return value / RPM_UNIT_SCALE;
  } else if (unit == "UNIT_DEGREE") {
    return value / DEGREE_UNIT_SCALE;
  } else if (unit == "UNIT_MILLI_AMPERE") {
    return value / MILLI_AMPERE_UNIT_SCALE;
  } else if (unit == "UNIT_ACCELERATION") {
    return value / MILLI_AMPERE_UNIT_SCALE;
  } else {

    return 0.0;  // 
  }
}

------------------Checkbaudrate.ino----------

void check_baudrate(uint8_t id, uint32_t baudrate) {
#define MAX_BAUD 6
  const int32_t baud1[MAX_BAUD] = { 57600, 115200, 1000000, 2000000, 3000000, 4000000 };

  int8_t index1 = 0;
  DEBUG_SERIAL.begin(57600);

  for (index1 = 0; index1 < MAX_BAUD; index1++) {

    DEBUG_SERIAL.print("SCAN BAUDRATE ");
    //DEBUG_SERIAL.print(baud1[index1]);
    // dxl.begin(baud1[index1]);

    if (dxl.ping(id)) {
      DEBUG_SERIAL.print("baud:");
      // DEBUG_SERIAL.print(baud1[index1]);
      DEBUG_SERIAL.print(" ID : ");
      DEBUG_SERIAL.print(id);
      DEBUG_SERIAL.print(", Model Number: ");
      DEBUG_SERIAL.println(dxl.getModelNumber(id));

      dxl.torqueOff(id);
      dxl.setBaudrate(id, baudrate);
      DEBUG_SERIAL.println("Baudrate has been successfully changed");
      dxl.begin(baudrate);
      break;
    }
  }
  DEBUG_SERIAL.println("DYNAMIXEL(s) found and baud rate(s) updated!");
}

I tested every method I could think of but couldn't find the cause of the problem.
Does anyone have any ideas on this interesting subject?

[david_2018]

You have commented out the dxl.begin() statement, then call functions that reference dxl before you reach the other dxl.begin() near the end of the Checkbaudrate.ino file.

Other than that, problems resulting from removing print() statements are usually the result of either writing to memory you do not own (such as writing outside the bounds of an array) somewhere else in the code, or the code depending on the delay caused by print() statements having to wait for space in the serial transmit buffer.

[zm476]

Hi @parworker can you try to just do a serial print command alone and tell us the result if it works than you don't have a problem with you're serial port it's just you're serial monitor and if it doesn't you have to reset the serial port

[parworker]

Hi,david_2018
I don't think this is the cause of the problem, because if I do not call this function and activate (// remove) the commands in the section I shared as a comment in the check_baudrate function, this problem occurs again. In other words, how can instructions in a function that is never called affect the execution of the code?

But I'm still thinking about the other things you said.

hi zm476

DEBUG_SERIAL.print("SCAN BAUDRATE ");
For example, if I test this command alone, there is no problem. It does not seem to be a problem with the serial monitor.
In other words, I can both send data and position and see the data on the serial monitor.the problem seems to be here baud1[index 1]
But I can't find exactly where the problem comes from.

[UKHeliBob]

I agree. The usual suspect for such memory corruption if an array index overflow where the array bounds have not been honoured. A rogue for loop is a common cause of this

[parworker]

Delta_G memory clogged is it possible ?

@UKHeliBob

I agree. The usual suspect for such memory corruption if an array index overflow where the array bounds have not been honoured. A rogue for loop is a common cause of this

#define MAX_BAUD 6 // >> **7**  
  const int32_t baud1[MAX_BAUD] = { 57600, 115200, 1000000, 2000000, 3000000, 4000000 };

When I changed this the problem went away, but I'm wondering why this fixed the problem.

[parworker]

@Delta_G

@parworker

I'm wondering why this fixed the problem

When chasing memory issues like this, you can't rely on the side effects of a change to let you know what's going on. This may have been the problem all along, or it may have simply resulted in things moving in memory a little and now the problem will show itself in a different way.

Everything seems to be working fine now, but this is really a problem. Is it a correct method to define the array as 7 elements even though it has 6 elements?

[camsysca]

@parworker

#define MAX_BAUD 6 // >> **7**  
  const int32_t baud1[MAX_BAUD] = { 57600, 115200, 1000000, 2000000, 3000000, 4000000 };

When I changed this the problem went away, but I'm wondering why this fixed the problem.

Umm, that's an array with 6 elements, indexed 0...5, just as it was in post #1.
So, what did you change? I'm missing something.

[parworker]

@camsysca

Umm, that's an array with 6 elements, indexed 0...5, just as it was in post #1.
So, what did you change? I'm missing something.

I changed this.

#define MAX_BAUD 7

[qubits-us]

@parworker

const uint8_t DXL_ID_CNT = 2;  //
const uint8_t DXL_ID_LIST[DXL_ID_CNT] = { SERVO_1_ID, SERVO_2_ID, /*SERVO_3_ID,SERVO_4_ID*/ };

left over, extra comma in the array constructor??

also inside check_bauderate you do DEBUG_SERIAL.begin again. already begun in setup..

good luck ~q

[parworker]

Delta_G You are right, I am worried that the unresolved symptom will change and I don't want to ignore the possibility of this problem as the code grows.

@qubits-us

@parworker

const uint8_t DXL_ID_CNT = 2;  //
const uint8_t DXL_ID_LIST[DXL_ID_CNT] = { SERVO_1_ID, SERVO_2_ID, /*SERVO_3_ID,SERVO_4_ID*/ };

left over, extra comma in the array constructor??

also inside check_bauderate you do DEBUG_SERIAL.begin again. already begun in setup..

good luck ~q

I changed it to like below

const uint8_t DXL_ID_LIST[DXL_ID_CNT] = { SERVO_1_ID, SERVO_2_ID }; 

and I removed the serial initialization in the baud rate control function

//DEBUG SERIAL.begin(57600);//Serial monitor

But it didn't solve the problem

[david_2018]

What board are you running this on?
The use of String in the function arguments for convertValue2Unit() and convertUnit2Value() might cause problems on a processor with a very limited amount of ram, and from a quick look at the code it seems an enum might be a suitable replacement.

[parworker]

@david_2018

What board are you running this on?
The use of String in the function arguments for convertValue2Unit() and convertUnit2Value() might cause problems on a processor with a very limited amount of ram, and from a quick look at the code it seems an enum might be a suitable replacement.

On the OpenCR1.0
-Do you think numbering would be beneficial?