byte Dynamixel::syncWrite(int start_addr, byte data_length, int *param, int param_length) {
int i=0, j=0, k=0, num=0;
if(mPacketType == DXL_PACKET_TYPE1) return 0;
this->clearBuffer();
num = param_length / (data_length + 1);
mParamBuffer[0] = DXL_LOBYTE(start_addr);
mParamBuffer[1] = DXL_HIBYTE(start_addr);
mParamBuffer[2] = DXL_LOBYTE(data_length*4);
mParamBuffer[3] = DXL_HIBYTE(data_length*4);
for(i=4; i < (4+num*(1+data_length*4)); i+=(1+data_length*4) ) {
mParamBuffer[i] = (byte)param[k++]; //ID
for(j=0; j < (data_length*4); j+=4) {
mParamBuffer[i+j+1] = DXL_LOBYTE(DXL_LOWORD(param[k])); //data
mParamBuffer[i+j+2] = DXL_HIBYTE(DXL_LOWORD(param[k]));
mParamBuffer[i+j+3] = DXL_LOBYTE(DXL_HIWORD(param[k]));
mParamBuffer[i+j+4] = DXL_HIBYTE(DXL_HIWORD(param[k]));
k++;
}
}
return this->txRxPacket(BROADCAST_ID, INST_SYNC_WRITE, 4+i);
}
byte Dynamixel::writeDword( byte bID, word wAddress, uint32 value ) {
this->clearBuffer();
if(mPacketType == DXL_PACKET_TYPE1) return 0;
//insert wAddress to parameter bucket
mParamBuffer[0] = (unsigned char)DXL_LOBYTE(wAddress);
mParamBuffer[1] = (unsigned char)DXL_HIBYTE(wAddress);
//insert data to parameter bucket
mParamBuffer[2] = DXL_LOBYTE(DXL_LOWORD(value));
mParamBuffer[3] = DXL_HIBYTE(DXL_LOWORD(value));
mParamBuffer[4] = DXL_LOBYTE(DXL_HIWORD(value));
mParamBuffer[5] = DXL_HIBYTE(DXL_HIWORD(value));
return this->txRxPacket(bID, INST_WRITE, 6); //// parameter length 4 = 2(address)+2(data)
}
uint8_t groupSyncWriteChangeParam(int group_num, uint8_t id, uint32_t data, uint16_t input_length, uint16_t data_pos)
{
int data_num = 0;
if (id == NOT_USED_ID) // NOT exist
return False;
data_num = find(group_num, id);
if (data_num == groupData[group_num].data_list_length)
return False;
if (data_pos + input_length > groupData[group_num].data_length)
return False;
switch (input_length)
{
case 1:
groupData[group_num].data_list[data_num].data[data_pos + 0] = DXL_LOBYTE(DXL_LOWORD(data));
break;
case 2:
groupData[group_num].data_list[data_num].data[data_pos + 0] = DXL_LOBYTE(DXL_LOWORD(data));
groupData[group_num].data_list[data_num].data[data_pos + 1] = DXL_HIBYTE(DXL_LOWORD(data));
break;
case 4:
groupData[group_num].data_list[data_num].data[data_pos + 0] = DXL_LOBYTE(DXL_LOWORD(data));
groupData[group_num].data_list[data_num].data[data_pos + 1] = DXL_HIBYTE(DXL_LOWORD(data));
groupData[group_num].data_list[data_num].data[data_pos + 2] = DXL_LOBYTE(DXL_HIWORD(data));
groupData[group_num].data_list[data_num].data[data_pos + 3] = DXL_HIBYTE(DXL_HIWORD(data));
break;
default:
return False;
}
groupData[group_num].is_param_changed = True;
return True;
}
int main()
{
// Initialize PortHandler instance
// Set the port path
// Get methods and members of PortHandlerLinux or PortHandlerWindows
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 GroupBulkWrite instance
dynamixel::GroupBulkWrite groupBulkWrite(portHandler, packetHandler);
// Initialize GroupBulkRead instance
dynamixel::GroupBulkRead groupBulkRead(portHandler, packetHandler);
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
int dxl_goal_position[2] = {DXL_MINIMUM_POSITION_VALUE, DXL_MAXIMUM_POSITION_VALUE}; // Goal position
uint8_t dxl_error = 0; // Dynamixel error
uint8_t dxl_led_value[2] = {0x00, 0xFF}; // Dynamixel LED value for write
uint8_t param_goal_position[4];
int32_t dxl1_present_position = 0; // Present position
uint8_t dxl2_led_value_read; // Dynamixel LED value for read
// Open port
if (portHandler->openPort())
{
printf("Succeeded to open the port!\n");
}
else
{
printf("Failed to open the port!\n");
printf("Press any key to terminate...\n");
getch();
return 0;
}
// Set port baudrate
if (portHandler->setBaudRate(BAUDRATE))
{
printf("Succeeded to change the baudrate!\n");
}
else
{
printf("Failed to change the baudrate!\n");
printf("Press any key to terminate...\n");
getch();
return 0;
}
// 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)
{
packetHandler->printTxRxResult(dxl_comm_result);
}
else if (dxl_error != 0)
{
packetHandler->printRxPacketError(dxl_error);
}
else
{
printf("DXL#%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)
{
packetHandler->printTxRxResult(dxl_comm_result);
}
else if (dxl_error != 0)
{
packetHandler->printRxPacketError(dxl_error);
}
else
{
printf("DXL#%d has been successfully connected \n", DXL2_ID);
}
// Add parameter storage for Dynamixel#1 present position
dxl_addparam_result = groupBulkRead.addParam(DXL1_ID, ADDR_PRO_PRESENT_POSITION, LEN_PRO_PRESENT_POSITION);
if (dxl_addparam_result != true)
{
fprintf(stderr, "[ID:%03d] grouBulkRead addparam failed", DXL1_ID);
return 0;
}
// Add parameter storage for Dynamixel#2 LED value
dxl_addparam_result = groupBulkRead.addParam(DXL2_ID, ADDR_PRO_LED_RED, LEN_PRO_LED_RED);
if (dxl_addparam_result != true)
{
fprintf(stderr, "[ID:%03d] grouBulkRead addparam failed", DXL2_ID);
return 0;
}
while(1)
{
printf("Press any key to continue! (or press ESC to quit!)\n");
if (getch() == ESC_ASCII_VALUE)
break;
// Allocate goal position value into byte array
param_goal_position[0] = DXL_LOBYTE(DXL_LOWORD(dxl_goal_position[index]));
param_goal_position[1] = DXL_HIBYTE(DXL_LOWORD(dxl_goal_position[index]));
param_goal_position[2] = DXL_LOBYTE(DXL_HIWORD(dxl_goal_position[index]));
param_goal_position[3] = DXL_HIBYTE(DXL_HIWORD(dxl_goal_position[index]));
// Add parameter storage for Dynamixel#1 goal position
dxl_addparam_result = groupBulkWrite.addParam(DXL1_ID, ADDR_PRO_GOAL_POSITION, LEN_PRO_GOAL_POSITION, param_goal_position);
if (dxl_addparam_result != true)
{
fprintf(stderr, "[ID:%03d] groupBulkWrite addparam failed", DXL1_ID);
return 0;
}
// Add parameter storage for Dynamixel#2 LED value
dxl_addparam_result = groupBulkWrite.addParam(DXL2_ID, ADDR_PRO_LED_RED, LEN_PRO_LED_RED, &dxl_led_value[index]);
if (dxl_addparam_result != true)
{
fprintf(stderr, "[ID:%03d] groupBulkWrite addparam failed", DXL2_ID);
return 0;
}
// Bulkwrite goal position and LED value
dxl_comm_result = groupBulkWrite.txPacket();
if (dxl_comm_result != COMM_SUCCESS) packetHandler->printTxRxResult(dxl_comm_result);
// Clear bulkwrite parameter storage
groupBulkWrite.clearParam();
do
{
// Bulkread present position and LED status
dxl_comm_result = groupBulkRead.txRxPacket();
if (dxl_comm_result != COMM_SUCCESS) packetHandler->printTxRxResult(dxl_comm_result);
// Check if groupbulkread data of Dynamixel#1 is available
dxl_getdata_result = groupBulkRead.isAvailable(DXL1_ID, ADDR_PRO_PRESENT_POSITION, LEN_PRO_PRESENT_POSITION);
if (dxl_getdata_result != true)
{
fprintf(stderr, "[ID:%03d] groupBulkRead getdata failed", DXL1_ID);
return 0;
}
// Check if groupbulkread data of Dynamixel#2 is available
dxl_getdata_result = groupBulkRead.isAvailable(DXL2_ID, ADDR_PRO_LED_RED, LEN_PRO_LED_RED);
if (dxl_getdata_result != true)
{
fprintf(stderr, "[ID:%03d] groupBulkRead getdata failed", DXL2_ID);
return 0;
}
// Get present position value
dxl1_present_position = groupBulkRead.getData(DXL1_ID, ADDR_PRO_PRESENT_POSITION, LEN_PRO_PRESENT_POSITION);
// Get LED value
dxl2_led_value_read = groupBulkRead.getData(DXL2_ID, ADDR_PRO_LED_RED, LEN_PRO_LED_RED);
printf("[ID:%03d] Present Position : %d \t [ID:%03d] LED Value: %d\n", DXL1_ID, dxl1_present_position, DXL2_ID, dxl2_led_value_read);
}while(abs(dxl_goal_position[index] - dxl1_present_position) > DXL_MOVING_STATUS_THRESHOLD);
// Change goal position
if (index == 0)
{
index = 1;
}
else
{
index = 0;
}
}
// Disable Dynamixel#1 Torque
dxl_comm_result = packetHandler->write1ByteTxRx(portHandler, DXL1_ID, ADDR_PRO_TORQUE_ENABLE, TORQUE_DISABLE, &dxl_error);
if (dxl_comm_result != COMM_SUCCESS)
{
packetHandler->printTxRxResult(dxl_comm_result);
}
else if (dxl_error != 0)
{
packetHandler->printRxPacketError(dxl_error);
}
// Disable Dynamixel#2 Torque
dxl_comm_result = packetHandler->write1ByteTxRx(portHandler, DXL2_ID, ADDR_PRO_TORQUE_ENABLE, TORQUE_DISABLE, &dxl_error);
if (dxl_comm_result != COMM_SUCCESS)
{
packetHandler->printTxRxResult(dxl_comm_result);
}
else if (dxl_error != 0)
{
packetHandler->printRxPacketError(dxl_error);
}
// Close port
portHandler->closePort();
return 0;
}
bool GroupBulkWrite_ChangeParam(int group_num, UINT8_T id, UINT16_T start_address, UINT16_T data_length, UINT32_T data, UINT16_T input_length, UINT16_T data_pos)
{
int _data_num = GroupBulkWrite_Find(group_num, id);
if (groupDataBulkWrite[group_num].protocol_version == 1)
return false;
if (id == NOT_USED_ID)
return false;
if (_data_num == groupDataBulkWrite[group_num].data_list_length_)
return false;
if (data_pos + input_length > groupDataBulkWrite[group_num].data_list_[_data_num].data_length_)
return false;
groupDataBulkWrite[group_num].data_list_[_data_num].data_length_ = data_length;
groupDataBulkWrite[group_num].data_list_[_data_num].start_address_ = start_address;
switch (input_length)
{
case 1:
groupDataBulkWrite[group_num].data_list_[_data_num].data_[data_pos + 0] = DXL_LOBYTE(DXL_LOWORD(data));
break;
case 2:
groupDataBulkWrite[group_num].data_list_[_data_num].data_[data_pos + 0] = DXL_LOBYTE(DXL_LOWORD(data));
groupDataBulkWrite[group_num].data_list_[_data_num].data_[data_pos + 1] = DXL_HIBYTE(DXL_LOWORD(data));
break;
case 4:
groupDataBulkWrite[group_num].data_list_[_data_num].data_[data_pos + 0] = DXL_LOBYTE(DXL_LOWORD(data));
groupDataBulkWrite[group_num].data_list_[_data_num].data_[data_pos + 1] = DXL_HIBYTE(DXL_LOWORD(data));
groupDataBulkWrite[group_num].data_list_[_data_num].data_[data_pos + 2] = DXL_LOBYTE(DXL_HIWORD(data));
groupDataBulkWrite[group_num].data_list_[_data_num].data_[data_pos + 3] = DXL_HIBYTE(DXL_HIWORD(data));
break;
default:
return false;
}
groupDataBulkWrite[group_num].is_param_changed_ = true;
return true;
}
bool GroupBulkWrite_AddParam(int group_num, UINT8_T id, UINT16_T start_address, UINT16_T data_length, UINT32_T data, UINT16_T input_length)
{
int _data_num = 0;
if (groupDataBulkWrite[group_num].protocol_version == 1)
return false;
if (id == NOT_USED_ID)
return false;
if (groupDataBulkWrite[group_num].data_list_length_ != 0)
_data_num = GroupBulkWrite_Find(group_num, id);
if (groupDataBulkWrite[group_num].data_list_length_ == _data_num)
{
groupDataBulkWrite[group_num].data_list_length_++;
groupDataBulkWrite[group_num].data_list_ = (DataListBulkWrite *)realloc(groupDataBulkWrite[group_num].data_list_, groupDataBulkWrite[group_num].data_list_length_ * sizeof(DataListBulkWrite));
groupDataBulkWrite[group_num].data_list_[_data_num].id_ = id;
groupDataBulkWrite[group_num].data_list_[_data_num].data_length_ = data_length;
groupDataBulkWrite[group_num].data_list_[_data_num].start_address_ = start_address;
groupDataBulkWrite[group_num].data_list_[_data_num].data_ = (UINT8_T *)calloc(groupDataBulkWrite[group_num].data_list_[_data_num].data_length_, sizeof(UINT8_T));
groupDataBulkWrite[group_num].data_list_[_data_num].data_end_ = 0;
}
else
if (groupDataBulkWrite[group_num].data_list_[_data_num].data_end_ + input_length > groupDataBulkWrite[group_num].data_list_[_data_num].data_length_)
return false;
switch (input_length)
{
case 1:
groupDataBulkWrite[group_num].data_list_[_data_num].data_[groupDataBulkWrite[group_num].data_list_[_data_num].data_end_ + 0] = DXL_LOBYTE(DXL_LOWORD(data));
break;
case 2:
groupDataBulkWrite[group_num].data_list_[_data_num].data_[groupDataBulkWrite[group_num].data_list_[_data_num].data_end_ + 0] = DXL_LOBYTE(DXL_LOWORD(data));
groupDataBulkWrite[group_num].data_list_[_data_num].data_[groupDataBulkWrite[group_num].data_list_[_data_num].data_end_ + 1] = DXL_HIBYTE(DXL_LOWORD(data));
break;
case 4:
groupDataBulkWrite[group_num].data_list_[_data_num].data_[groupDataBulkWrite[group_num].data_list_[_data_num].data_end_ + 0] = DXL_LOBYTE(DXL_LOWORD(data));
groupDataBulkWrite[group_num].data_list_[_data_num].data_[groupDataBulkWrite[group_num].data_list_[_data_num].data_end_ + 1] = DXL_HIBYTE(DXL_LOWORD(data));
groupDataBulkWrite[group_num].data_list_[_data_num].data_[groupDataBulkWrite[group_num].data_list_[_data_num].data_end_ + 2] = DXL_LOBYTE(DXL_HIWORD(data));
groupDataBulkWrite[group_num].data_list_[_data_num].data_[groupDataBulkWrite[group_num].data_list_[_data_num].data_end_ + 3] = DXL_HIBYTE(DXL_HIWORD(data));
break;
default:
return false;
}
groupDataBulkWrite[group_num].data_list_[_data_num].data_end_ = input_length;
groupDataBulkWrite[group_num].is_param_changed_ = true;
return true;
}
uint8_t groupSyncWriteAddParam(int group_num, uint8_t id, uint32_t data, uint16_t input_length)
{
int data_num = 0;
if (id == NOT_USED_ID)
return False;
if (groupData[group_num].data_list_length != 0)
data_num = find(group_num, id);
if (groupData[group_num].data_list_length == data_num)
{
groupData[group_num].data_list_length++;
groupData[group_num].data_list = (DataList *)realloc(groupData[group_num].data_list, groupData[group_num].data_list_length * sizeof(DataList));
groupData[group_num].data_list[data_num].id = id;
groupData[group_num].data_list[data_num].data = (uint8_t *)calloc(groupData[group_num].data_length, sizeof(uint8_t));
groupData[group_num].data_list[data_num].data_end = 0;
}
else
{
if (groupData[group_num].data_list[data_num].data_end + input_length > groupData[group_num].data_length)
return False;
}
switch (input_length)
{
case 1:
groupData[group_num].data_list[data_num].data[groupData[group_num].data_list[data_num].data_end + 0] = DXL_LOBYTE(DXL_LOWORD(data));
break;
case 2:
groupData[group_num].data_list[data_num].data[groupData[group_num].data_list[data_num].data_end + 0] = DXL_LOBYTE(DXL_LOWORD(data));
groupData[group_num].data_list[data_num].data[groupData[group_num].data_list[data_num].data_end + 1] = DXL_HIBYTE(DXL_LOWORD(data));
break;
case 4:
groupData[group_num].data_list[data_num].data[groupData[group_num].data_list[data_num].data_end + 0] = DXL_LOBYTE(DXL_LOWORD(data));
groupData[group_num].data_list[data_num].data[groupData[group_num].data_list[data_num].data_end + 1] = DXL_HIBYTE(DXL_LOWORD(data));
groupData[group_num].data_list[data_num].data[groupData[group_num].data_list[data_num].data_end + 2] = DXL_LOBYTE(DXL_HIWORD(data));
groupData[group_num].data_list[data_num].data[groupData[group_num].data_list[data_num].data_end + 3] = DXL_HIBYTE(DXL_HIWORD(data));
break;
default:
return False;
}
groupData[group_num].data_list[data_num].data_end = input_length;
groupData[group_num].is_param_changed = True;
return True;
}
int main()
{
// Initialize PortHandler instance
// Set the port path
// Get methods and members of PortHandlerLinux or PortHandlerWindows
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, ADDR_PRO_INDIRECTDATA_FOR_WRITE, LEN_PRO_INDIRECTDATA_FOR_WRITE);
// Initialize Groupsyncread instance
dynamixel::GroupSyncRead groupSyncRead(portHandler, packetHandler, ADDR_PRO_INDIRECTDATA_FOR_READ, LEN_PRO_INDIRECTDATA_FOR_READ);
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
int dxl_goal_position[2] = {DXL_MINIMUM_POSITION_VALUE, DXL_MAXIMUM_POSITION_VALUE}; // Goal position
uint8_t dxl_error = 0; // Dynamixel error
uint8_t dxl_moving = 0; // Dynamixel moving status
uint8_t param_indirect_data_for_write[LEN_PRO_INDIRECTDATA_FOR_WRITE];
uint8_t dxl_led_value[2] = {0x00, 0xFF}; // Dynamixel LED value
int32_t dxl_present_position = 0; // Present position
// Open port
if (portHandler->openPort())
{
printf("Succeeded to open the port!\n");
}
else
{
printf("Failed to open the port!\n");
printf("Press any key to terminate...\n");
getch();
return 0;
}
// Set port baudrate
if (portHandler->setBaudRate(BAUDRATE))
{
printf("Succeeded to change the baudrate!\n");
}
else
{
printf("Failed to change the baudrate!\n");
printf("Press any key to terminate...\n");
getch();
return 0;
}
// Disable Dynamixel Torque :
// Indirect address would not accessible when the torque is already enabled
dxl_comm_result = packetHandler->write1ByteTxRx(portHandler, DXL_ID, ADDR_PRO_TORQUE_ENABLE, TORQUE_DISABLE, &dxl_error);
if (dxl_comm_result != COMM_SUCCESS)
{
printf("%s\n", packetHandler->getTxRxResult(dxl_comm_result));
}
else if (dxl_error != 0)
{
printf("%s\n", packetHandler->getRxPacketError(dxl_error));
}
else
{
printf("DXL has been successfully connected \n");
}
// INDIRECTDATA parameter storages replace LED, goal position, present position and moving status storages
dxl_comm_result = packetHandler->write2ByteTxRx(portHandler, DXL_ID, ADDR_PRO_INDIRECTADDRESS_FOR_WRITE + 0, ADDR_PRO_GOAL_POSITION + 0, &dxl_error);
if (dxl_comm_result != COMM_SUCCESS)
{
printf("%s\n", packetHandler->getTxRxResult(dxl_comm_result));
}
else if (dxl_error != 0)
{
printf("%s\n", packetHandler->getRxPacketError(dxl_error));
}
dxl_comm_result = packetHandler->write2ByteTxRx(portHandler, DXL_ID, ADDR_PRO_INDIRECTADDRESS_FOR_WRITE + 2, ADDR_PRO_GOAL_POSITION + 1, &dxl_error);
if (dxl_comm_result != COMM_SUCCESS)
{
printf("%s\n", packetHandler->getTxRxResult(dxl_comm_result));
}
else if (dxl_error != 0)
{
printf("%s\n", packetHandler->getRxPacketError(dxl_error));
}
dxl_comm_result = packetHandler->write2ByteTxRx(portHandler, DXL_ID, ADDR_PRO_INDIRECTADDRESS_FOR_WRITE + 4, ADDR_PRO_GOAL_POSITION + 2, &dxl_error);
if (dxl_comm_result != COMM_SUCCESS)
{
printf("%s\n", packetHandler->getTxRxResult(dxl_comm_result));
}
else if (dxl_error != 0)
{
printf("%s\n", packetHandler->getRxPacketError(dxl_error));
}
dxl_comm_result = packetHandler->write2ByteTxRx(portHandler, DXL_ID, ADDR_PRO_INDIRECTADDRESS_FOR_WRITE + 6, ADDR_PRO_GOAL_POSITION + 3, &dxl_error);
if (dxl_comm_result != COMM_SUCCESS)
{
printf("%s\n", packetHandler->getTxRxResult(dxl_comm_result));
}
else if (dxl_error != 0)
{
printf("%s\n", packetHandler->getRxPacketError(dxl_error));
}
dxl_comm_result = packetHandler->write2ByteTxRx(portHandler, DXL_ID, ADDR_PRO_INDIRECTADDRESS_FOR_WRITE + 8, ADDR_PRO_LED_RED, &dxl_error);
if (dxl_comm_result != COMM_SUCCESS)
{
printf("%s\n", packetHandler->getTxRxResult(dxl_comm_result));
}
else if (dxl_error != 0)
{
printf("%s\n", packetHandler->getRxPacketError(dxl_error));
}
dxl_comm_result = packetHandler->write2ByteTxRx(portHandler, DXL_ID, ADDR_PRO_INDIRECTADDRESS_FOR_READ + 0, ADDR_PRO_PRESENT_POSITION + 0, &dxl_error);
if (dxl_comm_result != COMM_SUCCESS)
{
printf("%s\n", packetHandler->getTxRxResult(dxl_comm_result));
}
else if (dxl_error != 0)
{
printf("%s\n", packetHandler->getRxPacketError(dxl_error));
}
dxl_comm_result = packetHandler->write2ByteTxRx(portHandler, DXL_ID, ADDR_PRO_INDIRECTADDRESS_FOR_READ + 2, ADDR_PRO_PRESENT_POSITION + 1, &dxl_error);
if (dxl_comm_result != COMM_SUCCESS)
{
printf("%s\n", packetHandler->getTxRxResult(dxl_comm_result));
}
else if (dxl_error != 0)
{
printf("%s\n", packetHandler->getRxPacketError(dxl_error));
}
dxl_comm_result = packetHandler->write2ByteTxRx(portHandler, DXL_ID, ADDR_PRO_INDIRECTADDRESS_FOR_READ + 4, ADDR_PRO_PRESENT_POSITION + 2, &dxl_error);
if (dxl_comm_result != COMM_SUCCESS)
{
printf("%s\n", packetHandler->getTxRxResult(dxl_comm_result));
}
else if (dxl_error != 0)
{
printf("%s\n", packetHandler->getRxPacketError(dxl_error));
}
dxl_comm_result = packetHandler->write2ByteTxRx(portHandler, DXL_ID, ADDR_PRO_INDIRECTADDRESS_FOR_READ + 6, ADDR_PRO_PRESENT_POSITION + 3, &dxl_error);
if (dxl_comm_result != COMM_SUCCESS)
{
printf("%s\n", packetHandler->getTxRxResult(dxl_comm_result));
}
else if (dxl_error != 0)
{
printf("%s\n", packetHandler->getRxPacketError(dxl_error));
}
dxl_comm_result = packetHandler->write2ByteTxRx(portHandler, DXL_ID, ADDR_PRO_INDIRECTADDRESS_FOR_READ + 8, ADDR_PRO_MOVING, &dxl_error);
if (dxl_comm_result != COMM_SUCCESS)
{
printf("%s\n", packetHandler->getTxRxResult(dxl_comm_result));
}
else if (dxl_error != 0)
{
printf("%s\n", packetHandler->getRxPacketError(dxl_error));
}
// Enable Dynamixel Torque
dxl_comm_result = packetHandler->write1ByteTxRx(portHandler, DXL_ID, ADDR_PRO_TORQUE_ENABLE, TORQUE_ENABLE, &dxl_error);
if (dxl_comm_result != COMM_SUCCESS)
{
printf("%s\n", packetHandler->getTxRxResult(dxl_comm_result));
}
else if (dxl_error != 0)
{
printf("%s\n", packetHandler->getRxPacketError(dxl_error));
}
// Add parameter storage for the present position value
dxl_addparam_result = groupSyncRead.addParam(DXL_ID);
if (dxl_addparam_result != true)
{
fprintf(stderr, "[ID:%03d] groupSyncRead addparam failed\n", DXL_ID);
return 0;
}
while(1)
{
printf("Press any key to continue! (or press ESC to quit!)\n");
if (getch() == ESC_ASCII_VALUE)
break;
// Allocate LED and goal position value into byte array
param_indirect_data_for_write[0] = DXL_LOBYTE(DXL_LOWORD(dxl_goal_position[index]));
param_indirect_data_for_write[1] = DXL_HIBYTE(DXL_LOWORD(dxl_goal_position[index]));
param_indirect_data_for_write[2] = DXL_LOBYTE(DXL_HIWORD(dxl_goal_position[index]));
param_indirect_data_for_write[3] = DXL_HIBYTE(DXL_HIWORD(dxl_goal_position[index]));
param_indirect_data_for_write[4] = dxl_led_value[index];
// Add values to the Syncwrite storage
dxl_addparam_result = groupSyncWrite.addParam(DXL_ID, param_indirect_data_for_write);
if (dxl_addparam_result != true)
{
fprintf(stderr, "[ID:%03d] groupSyncWrite addparam failed\n", DXL_ID);
return 0;
}
// Syncwrite all
dxl_comm_result = groupSyncWrite.txPacket();
if (dxl_comm_result != COMM_SUCCESS) printf("%s\n", packetHandler->getTxRxResult(dxl_comm_result));
// Clear syncwrite parameter storage
groupSyncWrite.clearParam();
do
{
// Syncread present position from indirectdata2
dxl_comm_result = groupSyncRead.txRxPacket();
if (dxl_comm_result != COMM_SUCCESS) printf("%s\n", packetHandler->getTxRxResult(dxl_comm_result));
// Check if groupsyncread data of Dyanamixel is available
dxl_getdata_result = groupSyncRead.isAvailable(DXL_ID, ADDR_PRO_INDIRECTDATA_FOR_READ, LEN_PRO_PRESENT_POSITION);
if (dxl_getdata_result != true)
{
fprintf(stderr, "[ID:%03d] groupSyncRead getdata failed", DXL_ID);
return 0;
}
// Check if groupsyncread data of Dyanamixel is available
dxl_getdata_result = groupSyncRead.isAvailable(DXL_ID, ADDR_PRO_INDIRECTDATA_FOR_READ + LEN_PRO_PRESENT_POSITION, LEN_PRO_MOVING);
if (dxl_getdata_result != true)
{
fprintf(stderr, "[ID:%03d] groupSyncRead getdata failed", DXL_ID);
return 0;
}
// Get Dynamixel present position value
dxl_present_position = groupSyncRead.getData(DXL_ID, ADDR_PRO_INDIRECTDATA_FOR_READ, LEN_PRO_PRESENT_POSITION);
// Get Dynamixel moving status value
dxl_moving = groupSyncRead.getData(DXL_ID, ADDR_PRO_INDIRECTDATA_FOR_READ + LEN_PRO_PRESENT_POSITION, LEN_PRO_MOVING);
printf("[ID:%03d] GoalPos:%d PresPos:%d IsMoving:%d\n", DXL_ID, dxl_goal_position[index], dxl_present_position, dxl_moving);
}while(abs(dxl_goal_position[index] - dxl_present_position) > DXL_MOVING_STATUS_THRESHOLD);
// Change goal position
if (index == 0)
{
index = 1;
}
else
{
index = 0;
}
}
// Disable Dynamixel Torque
dxl_comm_result = packetHandler->write1ByteTxRx(portHandler, DXL_ID, ADDR_PRO_TORQUE_ENABLE, TORQUE_DISABLE, &dxl_error);
if (dxl_comm_result != COMM_SUCCESS)
{
printf("%s\n", packetHandler->getTxRxResult(dxl_comm_result));
}
else if (dxl_error != 0)
{
printf("%s\n", packetHandler->getRxPacketError(dxl_error));
}
// Close port
portHandler->closePort();
return 0;
}