Dynmaixel sdk + goal velocity

Issue Description

Hi everyone, i am trying to write a motor controller driver from scratch using my Turtlebot3 dyanmixel motors and opencr.

Am currently using arduino IDE and writing it in c via my opencr using protcol ver 2.0

The issue i am facing is that i am having difficulty driving both motors using goal velocity via syncwrite, just want to drive both motors as a first step.

When i input an negative int64_t cmd_vel regardless of any number from -1 to -255 it will just go at full speed. When i insert a positive number, both motors will not move at all. Just want to able to control the velocity of the motors forward and backward. Am not sure why i am facing the above issue

I am really not sure what else to change as i reference the code from Turtlebot3 opencr
https://github.com/ROBOTIS-GIT/OpenCR/blob/master/arduino/opencr_arduino/opencr/libraries/turtlebot3/src/turtlebot3/turtlebot3_motor_driver.cpp

Additional Information/Attachments

This is a sample of my code

#include <DynamixelSDK.h>



// Control table address
#define ADDR_PRO_TORQUE_ENABLE          64                 // Control table address is different in Dynamixel model
#define ADDR_PRO_GOAL_POSITION          116
#define ADDR_PRO_PRESENT_POSITION       132
#define OPERATING_MODE                  11                 // Operating mode 11, Velocity control ->1
#define GOAL_VELOCITY                   104                //desired Velocity choice 

// Data Byte Length
#define LEN_GOAL_VELOCITY                4



// Protocol version
#define PROTOCOL_VERSION                2.0                 // See which protocol version is used in the Dynamixel

// Default setting
#define DXL1_ID                         1                   // Dynamixel#1 ID: 1
#define DXL2_ID                         2                   // Dynamixel#2 ID: 2
#define BAUDRATE                        1000000
#define DEVICENAME                      "OpenCR_DXL_Port"   // This definition only has a symbolic meaning and does not affect to any functionality

#define TORQUE_ENABLE                   1                   // Value for enabling the torque
#define TORQUE_DISABLE                  0                   // Value for disabling the torque

#define ESC_ASCII_VALUE                 0x1b

#define CMD_SERIAL                      Serial

int getch()
{
  while(1)
  {
    if( CMD_SERIAL.available() > 0 )
    {
      break;
    }
  }

  return CMD_SERIAL.read();
}

int kbhit(void)
{
  return CMD_SERIAL.available();
}

void setup()
{
  Serial.begin(115200);
  while(!Serial);

  

  Serial.println("Start..");
  dynamixel::PortHandler *portHandler = dynamixel::PortHandler::getPortHandler(DEVICENAME);

  // Initialize PacketHandler instance
  // Set the protocol version
  // Get methods and members of Protocol1PacketHandler or Protocol2PacketHandler
  dynamixel::PacketHandler *packetHandler = dynamixel::PacketHandler::getPacketHandler(PROTOCOL_VERSION);

  // Initialize GroupSyncWrite instance
    dynamixel::GroupSyncWrite groupSyncWrite(portHandler, packetHandler, GOAL_VELOCITY, LEN_GOAL_VELOCITY);


  int index = 0;
  int dxl_comm_result = COMM_TX_FAIL;             // Communication result
  bool dxl_addparam_result = false;                // addParam result
  bool dxl_getdata_result = false;                 // GetParam result

  uint8_t dxl_error = 0;                          // Dynamixel error
  uint8_t param_goal_velocity[4]={0,};
  int64_t cmd_vel = -100;


  // Open port
  if (portHandler->openPort())
  {
  
  }
  else
  {

  }

  // Set port baudrate
  if (portHandler->setBaudRate(BAUDRATE))
  {
    
  }
  else
  {

  }

  // Operating Mode
  // 3 position mode
  // 1 velocity mode
  dxl_comm_result = packetHandler->write1ByteTxRx(portHandler, DXL1_ID, OPERATING_MODE, 1, &dxl_error);
  dxl_comm_result = packetHandler->write1ByteTxRx(portHandler, DXL2_ID, OPERATING_MODE, 1, &dxl_error);

  // Enable Dynamixel#1 Torque
  dxl_comm_result = packetHandler->write1ByteTxRx(portHandler, DXL1_ID, ADDR_PRO_TORQUE_ENABLE, TORQUE_ENABLE, &dxl_error);
  if (dxl_comm_result != COMM_SUCCESS)
  {
    
  }
  else if (dxl_error != 0)
  {
    
  }
  else
  {
    //printf("Dynamixel#%d has been successfully connected \n", DXL1_ID);
  }

  // Enable Dynamixel#2 Torque
  dxl_comm_result = packetHandler->write1ByteTxRx(portHandler, DXL2_ID, ADDR_PRO_TORQUE_ENABLE, TORQUE_ENABLE, &dxl_error);
  if (dxl_comm_result != COMM_SUCCESS)
  {
    
  }
  else if (dxl_error != 0)
  {
    
  }
  else
  {
    //printf("Dynamixel#%d has been successfully connected \n", DXL2_ID);
  }
      

    Serial.print("Press any key to continue!\n");
    if (getch() == ESC_ASCII_VALUE)
    

    // Allocate velocity -> goal_velocity -why does negative cmd_vel go to max spd and positive cmd_vel doesn't move
    param_goal_velocity[0] = DXL_LOBYTE(DXL_LOWORD(cmd_vel));
    param_goal_velocity[1] = DXL_HIBYTE(DXL_LOWORD(cmd_vel));
    param_goal_velocity[2] = DXL_LOBYTE(DXL_HIWORD(cmd_vel));
    param_goal_velocity[3] = DXL_HIBYTE(DXL_HIWORD(cmd_vel));

    // Add Dynamixel#1 goal position value to the Syncwrite storage
    dxl_addparam_result = groupSyncWrite.addParam(DXL1_ID,(uint8_t*)&param_goal_velocity); //4 bytes array
    if (dxl_addparam_result != true)
    {
      
    }

    // Add Dynamixel#2 goal position value to the Syncwrite parameter storage
    dxl_addparam_result = groupSyncWrite.addParam(DXL2_ID,(uint8_t*)&param_goal_velocity);
    if (dxl_addparam_result != true)
    {

    }

    // Syncwrite goal position
    dxl_comm_result = groupSyncWrite.txPacket();
    if (dxl_comm_result != COMM_SUCCESS)
    {

    } 

    Serial.print("Press any key to continue! (or press q to quit!)\n");

    while(1)
    {
      if (getch() == 'q')
      {
        groupSyncWrite.clearParam();
        //set velocity to zero
        param_goal_velocity[0] = DXL_LOBYTE(DXL_LOWORD(0));
        param_goal_velocity[1] = DXL_HIBYTE(DXL_LOWORD(0));
        param_goal_velocity[2] = DXL_LOBYTE(DXL_HIWORD(0));
        param_goal_velocity[3] = DXL_HIBYTE(DXL_HIWORD(0));
    
    
        // Add Dynamixel#1 goal position value to the Syncwrite storage
        dxl_addparam_result = groupSyncWrite.addParam(DXL1_ID,(uint8_t*)&param_goal_velocity); //4 bytes array
        if (dxl_addparam_result != true)
        {
          
        }
    
        // Add Dynamixel#2 goal position value to the Syncwrite parameter storage
        dxl_addparam_result = groupSyncWrite.addParam(DXL2_ID,(uint8_t*)&param_goal_velocity);
        if (dxl_addparam_result != true)
        {
    
        }
    
        // Syncwrite goal position
        dxl_comm_result = groupSyncWrite.txPacket();
        if (dxl_comm_result != COMM_SUCCESS)
        {
    
        } 
        
      }

    
    

    } 

    
      

    

    // Clear syncwrite parameter storage
    groupSyncWrite.clearParam();

   
}

void loop()
{
}

What model of actuator are you using?