byte Dynamixel::syncWrite(byte start_addr, byte num_of_data, int *param, int array_length){
int i,j,k,num;
k=0;
num = array_length/(1+num_of_data); //ID+DATA1+DATA2..
mbLengthForPacketMaking = 0;
mbIDForPacketMaking = BROADCAST_ID;
mbInstructionForPacketMaking = INST_SYNC_WRITE;
mCommStatus = 0;
mParamBuffer[mbLengthForPacketMaking++] = start_addr;
mParamBuffer[mbLengthForPacketMaking++] = num_of_data*2;
for(i=mbLengthForPacketMaking; i < num*(1+num_of_data*2); i+=(1+num_of_data*2)){
mParamBuffer[i] = param[k++]; //ID
for(j=0; j < (num_of_data*2); j+=2){
mParamBuffer[i+j+1] = (unsigned char)DXL_LOBYTE(param[k]); //low byte
mParamBuffer[i+j+2] = (unsigned char)DXL_HIBYTE(param[k]);; //high byte
k++;
}
}
mbLengthForPacketMaking= i;
mCommStatus = this->txRxPacket(mbIDForPacketMaking, mbInstructionForPacketMaking, mbLengthForPacketMaking);
return mCommStatus;
}
int MX_controller::SetGoalPosition(int *GoalPosition){
unsigned char *t_param;
t_param = (unsigned char*)malloc(sizeof(unsigned char)*(NUM_FINGER*3));
for(int i = 0; i < NUM_FINGER; i++){
t_param[i*3] = (unsigned char)ID_list_[i];
t_param[i*3+1] = DXL_LOBYTE(DXL_LOWORD(GoalPosition[i]));
t_param[i*3+2] = DXL_HIBYTE(DXL_LOWORD(GoalPosition[i]));
/*t_param[i*5+3] = DXL_LOBYTE(DXL_HIWORD(GoalPosition[i]));
t_param[i*5+4] = DXL_HIBYTE(DXL_HIWORD(GoalPosition[i]));*/
}
int Result = dxl_sync_write(Port_, MX_GOAL_POSITION, 2, t_param, NUM_FINGER*3);
if( Result != COMM_RXSUCCESS )
{
printf( "Failed to set goal position!\n" );
free(t_param);
return -1;
}
free(t_param);
return 1;
}
word Dynamixel::readWord(byte bID, word bAddress) {
this->clearBuffer();
if(mPacketType == DXL_PACKET_TYPE1) {
mParamBuffer[0] = bAddress;
mParamBuffer[1] = 2;
if(this->txRxPacket(bID, INST_READ, 2)) {
return DXL_MAKEWORD(mRxBuffer[5],mRxBuffer[6]);//( (((word)mRxBuffer[6])<<8)+ mRxBuffer[5] );
}
else {
return 0xffff;
}
} else {
mParamBuffer[0] = (unsigned char)DXL_LOBYTE(bAddress);
mParamBuffer[1] = (unsigned char)DXL_HIBYTE(bAddress);
mParamBuffer[2] = 2; //2byte
mParamBuffer[3] = 0;
if(this->txRxPacket(bID, INST_READ, 4)) {
return(DXL_MAKEWORD(mRxBuffer[9], mRxBuffer[10]));
} else {
return 0xffff;
}
}
}
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_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;
}