Below is my own example code to read the “Present Position” which never cause the freeze in my case. I wanted to look into your code and find a root cause, but as I have no understanding your code unless I put my resources and time much, nor it’s been long time I used the C/C++, it is difficult for me to look into further. I highly recommend replacing your dynamic allocation method with just normal data type for test only and see if the freeze is caused by the memory leakeage by the dynamic memory allocation (using new operator). (*As far as I know, you should free your heap memory (using delete operator) if not using such addresses)
#include <Dynamixel2Arduino.h>
#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 int DXL_DIR_PIN = 22; //OpenCM9.04 EXP Board's DIR PIN. (28 for the DXL port on the OpenCM 9.04 board)
//Please see eManual Control Table section of your DYNAMIXEL.
//This example is written for DYNAMIXEL AX & MX series with Protocol 1.0.
//For MX 2.0 with Protocol 2.0, refer to write_x.ino example.
#define CW_ANGLE_LIMIT_ADDR 6
#define CCW_ANGLE_LIMIT_ADDR 8
#define ANGLE_LIMIT_ADDR_LEN 2
#define OPERATING_MODE_ADDR_LEN 2
#define TORQUE_ENABLE_ADDR 24
#define TORQUE_ENABLE_ADDR_LEN 1
#define MOVING_SPEED_ADDR 32
#define MOVING_SPEED_ADDR_LEN 2
#define LED_ADDR 25
#define LED_ADDR_LEN 1
#define GOAL_POSITION_ADDR 30
#define GOAL_POSITION_ADDR_LEN 2
#define PRESENT_POSITION_ADDR 36
#define PRESENT_POSITION_ADDR_LEN 2
#define TIMEOUT 10 //default communication timeout 10ms
uint8_t turn_on = 1;
uint8_t turn_off = 0;
uint16_t present_position = 0;
uint set_speed = 200;
const uint8_t DXL_ID = 1;
const float DXL_PROTOCOL_VERSION = 1.0;
uint16_t goalPosition1 = 0;
uint16_t goalPosition2 = 1023;
unsigned long current_time = 0;
unsigned long saved_time = 0;
Dynamixel2Arduino dxl(DXL_SERIAL, DXL_DIR_PIN);
void setup() {
// put your setup code here, to run once:
// For Uno, Nano, Mini, and Mega, use UART port of DYNAMIXEL Shield to debug.
DEBUG_SERIAL.begin(115200); //Set debugging port baudrate to 115200bps
while(!DEBUG_SERIAL); //Wait until the serial port for terminal is opened
// Set Port baudrate to 57600bps. This has to match with DYNAMIXEL baudrate.
dxl.begin(1000000);
// Set Port Protocol Version. This has to match with DYNAMIXEL protocol version.
dxl.setPortProtocolVersion(DXL_PROTOCOL_VERSION);
// Turn off torque when configuring items in EEPROM area
if(dxl.write(DXL_ID, TORQUE_ENABLE_ADDR, (uint8_t*)&turn_off , TORQUE_ENABLE_ADDR_LEN, TIMEOUT))
DEBUG_SERIAL.println("DYNAMIXEL Torque off");
else
DEBUG_SERIAL.println("Error: Torque off failed");
delay(100);
// Set to Joint Mode
if(dxl.write(DXL_ID, CW_ANGLE_LIMIT_ADDR, (uint8_t*)&goalPosition1, ANGLE_LIMIT_ADDR_LEN, TIMEOUT)
&& dxl.write(DXL_ID, CCW_ANGLE_LIMIT_ADDR, (uint8_t*)&goalPosition2, ANGLE_LIMIT_ADDR_LEN, TIMEOUT))
DEBUG_SERIAL.println("Set operating mode");
else
DEBUG_SERIAL.println("Error: Set operating mode failed");
delay(100);
// Turn on torque
if(dxl.write(DXL_ID, TORQUE_ENABLE_ADDR, (uint8_t*)&turn_on, TORQUE_ENABLE_ADDR_LEN, TIMEOUT))
DEBUG_SERIAL.println("Torque on");
else
DEBUG_SERIAL.println("Error: Torque on failed");
delay(100);
if(dxl.write(DXL_ID, MOVING_SPEED_ADDR, (uint8_t*)&set_speed, MOVING_SPEED_ADDR_LEN, TIMEOUT))
DEBUG_SERIAL.println("SET SPEED");
else
DEBUG_SERIAL.println("Error: SET SPEED failed");
delay(100);
}
void loop() {
DEBUG_SERIAL.print("Goal Position : ");
DEBUG_SERIAL.println(goalPosition1);
dxl.write(DXL_ID, GOAL_POSITION_ADDR, (uint8_t*)&goalPosition1, GOAL_POSITION_ADDR_LEN, TIMEOUT);
while((current_time - saved_time) <= 5000){
// Read DYNAMIXEL Present Position
current_time = millis();
DEBUG_SERIAL.print("Present Position : ");
// dxl.read(DXL_ID, PRESENT_POSITION_ADDR, PRESENT_POSITION_ADDR_LEN, (uint8_t*)&present_position, sizeof(present_position), TIMEOUT);
present_position = dxl.getPresentPosition(DXL_ID);
DEBUG_SERIAL.println(present_position);
}
saved_time = current_time;
// // Set Goal Position
DEBUG_SERIAL.print("Goal Position : ");
DEBUG_SERIAL.println(goalPosition2);
dxl.write(DXL_ID, GOAL_POSITION_ADDR, (uint8_t*)&goalPosition2, GOAL_POSITION_ADDR_LEN, TIMEOUT);
while((current_time - saved_time) <= 5000){
// Read DYNAMIXEL Present Position
current_time = millis();
DEBUG_SERIAL.print("Present Position : ");
// dxl.read(DXL_ID, PRESENT_POSITION_ADDR, PRESENT_POSITION_ADDR_LEN, (uint8_t*)&present_position, sizeof(present_position), TIMEOUT);
present_position = dxl.getPresentPosition(DXL_ID);
DEBUG_SERIAL.println(present_position);
}
saved_time = current_time;
}
