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*)¶m_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*)¶m_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*)¶m_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*)¶m_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()
{
}