void GroupBulkWrite_MakeParam(int group_num)
{
int _data_num, _idx, _c;
int _port_num = groupDataBulkWrite[group_num].port_num;
if (groupDataBulkWrite[group_num].protocol_version == 1)
return;
if (GroupBulkWrite_Size(group_num) == 0)
return;
groupDataBulkWrite[group_num].param_length_ = 0;
_idx = 0;
for (_data_num = 0; _data_num < groupDataBulkWrite[group_num].data_list_length_; _data_num++)
{
if (groupDataBulkWrite[group_num].data_list_[_data_num].id_ == NOT_USED_ID)
continue;
groupDataBulkWrite[group_num].param_length_ += 1 + 2 + 2 + groupDataBulkWrite[group_num].data_list_[_data_num].data_length_;
packetData[_port_num].data_write_ = (UINT8_T*)realloc(packetData[_port_num].data_write_, groupDataBulkWrite[group_num].param_length_ * sizeof(UINT8_T));
packetData[_port_num].data_write_[_idx++] = groupDataBulkWrite[group_num].data_list_[_data_num].id_;
packetData[_port_num].data_write_[_idx++] = DXL_LOBYTE(groupDataBulkWrite[group_num].data_list_[_data_num].start_address_);
packetData[_port_num].data_write_[_idx++] = DXL_HIBYTE(groupDataBulkWrite[group_num].data_list_[_data_num].start_address_);
packetData[_port_num].data_write_[_idx++] = DXL_LOBYTE(groupDataBulkWrite[group_num].data_list_[_data_num].data_length_);
packetData[_port_num].data_write_[_idx++] = DXL_HIBYTE(groupDataBulkWrite[group_num].data_list_[_data_num].data_length_);
for (_c = 0; _c < groupDataBulkWrite[group_num].data_list_[_data_num].data_length_; _c++)
packetData[_port_num].data_write_[_idx++] = groupDataBulkWrite[group_num].data_list_[_data_num].data_[_c];
}
}
int MX_controller::GetGoalPosition(int *GoalPosition){
int tG_position[NUM_FINGER],tPosition_length = 2;
unsigned char *t_param;
t_param = (unsigned char*)malloc(sizeof(unsigned char)*(NUM_FINGER*5));
for(int i = 0; i < NUM_FINGER; i++){
t_param[i*5] = (unsigned char)ID_list_[i];
t_param[i*5+1] = DXL_LOBYTE(MX_GOAL_POSITION);
t_param[i*5+2] = DXL_HIBYTE(MX_GOAL_POSITION);
t_param[i*5+3] = DXL_LOBYTE(tPosition_length);
t_param[i*5+4] = DXL_HIBYTE(tPosition_length);
}
dxl_bulk_read(Port_, t_param, NUM_FINGER*5, pbd);
for(int i = 0; i < NUM_FINGER; i++){
dxl_get_bulk_word(pbd, ID_list_[i], MX_GOAL_POSITION, &tG_position[i]);
if(GoalPosition != NULL){
GoalPosition[i] = tG_position[i];
}else
printf("ID %d : %d\n", ID_list_[i], tG_position[i]);
}
free(t_param);
return 1;
}
int MX_controller::GetGoalVelocity(int *GoalVelocity){
int tG_velocity[NUM_FINGER],tVelocity_length = 2;
unsigned char *t_param;
t_param = (unsigned char*)malloc(sizeof(unsigned char)*(NUM_FINGER*5));
for(int i = 0; i < NUM_FINGER; i++){
t_param[i*5] = (unsigned char)ID_list_[i];
t_param[i*5+1] = DXL_LOBYTE(MX_MOVING_SPEED);
t_param[i*5+2] = DXL_HIBYTE(MX_MOVING_SPEED);
t_param[i*5+3] = DXL_LOBYTE(tVelocity_length);
t_param[i*5+4] = DXL_HIBYTE(tVelocity_length);
}
dxl_bulk_read(Port_, t_param, NUM_FINGER*5, pbd);
for(int i = 0; i < NUM_FINGER; i++){
dxl_get_bulk_word(pbd, ID_list_[i], MX_MOVING_SPEED, &tG_velocity[i]);
if(GoalVelocity != NULL){
GoalVelocity[i] = tG_velocity[i];
}else
printf("ID %d : %d\n", ID_list_[i], tG_velocity[i]);
}
free(t_param);
return 1;
}
int MX_controller::isMoving(bool *ismove){
int tP_position[NUM_FINGER],tIsMovingLength = 1;
unsigned char *t_param;
t_param = (unsigned char*)malloc(sizeof(unsigned char)*(NUM_FINGER*5));
for(int i = 0; i < NUM_FINGER; i++){
t_param[i*5] = (unsigned char)ID_list_[i];
t_param[i*5+1] = DXL_LOBYTE(MX_MOVING);
t_param[i*5+2] = DXL_HIBYTE(MX_MOVING);
t_param[i*5+3] = DXL_LOBYTE(tIsMovingLength);
t_param[i*5+4] = DXL_HIBYTE(tIsMovingLength);
}
dxl_bulk_read(Port_, t_param, NUM_FINGER*5, pbd);
for(int i = 0; i < NUM_FINGER; i++){
dxl_get_bulk_byte(pbd, ID_list_[i], MX_MOVING, &tP_position[i]);
if(ismove != NULL){
ismove[i] = tP_position[i] == 1 ? true : false;
}else
printf("ID %d : %d\n", ID_list_[i], tP_position[i]);
}
free(t_param);
return 1;
}
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);
}
int MX_controller::GetPresLoad(int *PresentLoad){
int tP_load[NUM_FINGER],tLoad_length = 2;
unsigned char *t_param;
t_param = (unsigned char*)malloc(sizeof(unsigned char)*(NUM_FINGER*5));
for(int i = 0; i < NUM_FINGER; i++){
t_param[i*5] = (unsigned char)ID_list_[i];
t_param[i*5+1] = DXL_LOBYTE(MX_PRESENT_LOAD);
t_param[i*5+2] = DXL_HIBYTE(MX_PRESENT_LOAD);
t_param[i*5+3] = DXL_LOBYTE(tLoad_length);
t_param[i*5+4] = DXL_HIBYTE(tLoad_length);
}
while(1){
dxl_bulk_read(Port_, t_param, NUM_FINGER*5, pbd);
for(int i = 0; i < NUM_FINGER; i++){
dxl_get_bulk_word(pbd, ID_list_[i], MX_PRESENT_LOAD, &tP_load[i]);
if(PresentLoad != NULL){
PresentLoad[i] = tP_load[i];
}else
printf("ID %d : %d\n", ID_list_[i], tP_load[i]);
}
if(DataValidCheck(tP_load, 2048, 0) == 1)
break;
}
free(t_param);
return 1;
}
int MX_controller::GetTemperature(int *Temperature){
int tP_temperature[NUM_FINGER], temperature_length = 1;
unsigned char *t_param;
t_param = (unsigned char*)malloc(sizeof(unsigned char)*(NUM_FINGER*5));
for(int i = 0; i < NUM_FINGER; i++){
t_param[i*5] = (unsigned char)ID_list_[i];
t_param[i*5+1] = DXL_LOBYTE(MX_PRESENT_TEMPERATURE);
t_param[i*5+2] = DXL_HIBYTE(MX_PRESENT_TEMPERATURE);
t_param[i*5+3] = DXL_LOBYTE(temperature_length);
t_param[i*5+4] = DXL_HIBYTE(temperature_length);
}
dxl_bulk_read(Port_, t_param, NUM_FINGER*5, pbd);
for(int i = 0; i < NUM_FINGER; i++){
dxl_get_bulk_byte(pbd, ID_list_[i], MX_PRESENT_TEMPERATURE, &tP_temperature[i]);
if(Temperature != NULL){
Temperature[i] = tP_temperature[i];
}else
printf("ID %d : %d\n", ID_list_[i], tP_temperature[i]);
}
free(t_param);
return 1;
}
byte Dynamixel::txPacket(byte bID, byte bInstruction, int bParameterLength) {
word bCount,bCheckSum,bPacketLength;
byte offsetParamIndex;
if(mPacketType == DXL_PACKET_TYPE1) { //dxl protocol 1.0
mTxBuffer[0] = 0xff;
mTxBuffer[1] = 0xff;
mTxBuffer[2] = bID;
mTxBuffer[3] = bParameterLength+2; //2(byte) <- instruction(1byte) + checksum(1byte)
mTxBuffer[4] = bInstruction;
offsetParamIndex = 5;
bPacketLength = bParameterLength+2+4;
} else { //dxl protocol 2.0
mTxBuffer[0] = 0xff;
mTxBuffer[1] = 0xff;
mTxBuffer[2] = 0xfd;
mTxBuffer[3] = 0x00;
mTxBuffer[4] = bID;
//get parameter length
mTxBuffer[5] = DXL_LOBYTE(bParameterLength+3);// 3(byte) <- instruction(1byte) + checksum(2byte)
mTxBuffer[6] = DXL_HIBYTE(bParameterLength+3);
mTxBuffer[7] = bInstruction;
offsetParamIndex = 8;
bPacketLength = bParameterLength+3+7; //parameter length 3bytes, 7bytes = packet header 4bytes, ID 1byte, length 2bytes
}
//copy parameters from mParamBuffer to mTxBuffer
for(bCount = 0; bCount < bParameterLength; bCount++)
{
mTxBuffer[bCount+offsetParamIndex] = mParamBuffer[bCount];
}
if(mPacketType == DXL_PACKET_TYPE1) {
bCheckSum = 0;
for(bCount = 2; bCount < bPacketLength-1; bCount++) { //except 0xff,checksum
bCheckSum += mTxBuffer[bCount];
}
mTxBuffer[bCount] = ~bCheckSum; //Writing Checksum with Bit Inversion
} else {
bCheckSum = update_crc(0, mTxBuffer, bPacketLength-2); // -2 : except CRC16
mTxBuffer[bPacketLength-2] = DXL_LOBYTE(bCheckSum); // last - 2
mTxBuffer[bPacketLength-1] = DXL_HIBYTE(bCheckSum); // last - 1
}
//TxDStringC("bPacketLength = ");TxDHex8C(bPacketLength);TxDStringC("\r\n");
this->dxlTxEnable(); // this define is declared in dxl.h
for(bCount = 0; bCount < bPacketLength; bCount++)
{
writeRaw(mTxBuffer[bCount]);
}
this->dxlTxDisable();// this define is declared in dxl.h
return(bPacketLength); // return packet length
}
/*
* @brief Sets the target position and speed of the specified servo
* @author Made by Martin S. Mason(Professor @Mt. San Antonio College)
* @change 2013-04-17 changed by ROBOTIS,.LTD.
* */
byte Dynamixel::setPosition(byte ServoID, int Position, int Speed){
mParamBuffer[0] = (unsigned char)30;
mParamBuffer[1] = (unsigned char)DXL_LOBYTE(Position);
mParamBuffer[2] = (unsigned char)DXL_HIBYTE(Position);
mParamBuffer[3] = (unsigned char)DXL_LOBYTE(Speed);
mParamBuffer[4] = (unsigned char)DXL_HIBYTE(Speed);
return(this->txRxPacket(ServoID, INST_WRITE, 5));
}
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)
}
byte Dynamixel::readByte(byte bID, word bAddress) {
this->clearBuffer();
if(mPacketType == DXL_PACKET_TYPE1) {
mParamBuffer[0] = bAddress;
mParamBuffer[1] = 1;
if( this->txRxPacket(bID, INST_READ, 2 )) {
//mCommStatus = 1;
return(mRxBuffer[5]); //refer to 1.0 packet structure
}
else {
//mCommStatus = 0;
return 0xff;
}
} else {
mParamBuffer[0] = (unsigned char)DXL_LOBYTE(bAddress);
mParamBuffer[1] = (unsigned char)DXL_HIBYTE(bAddress);
mParamBuffer[2] = 1; //1byte
mParamBuffer[3] = 0;
if( this->txRxPacket(bID, INST_READ, 4 )) {
return(mRxBuffer[9]);//refer to 2.0 packet structure
}
else {
return 0xff;
}
}
}
void Write2ByteTxRx1(int port_num, UINT8_T id, UINT16_T address, UINT16_T data)
{
packetData[port_num].data_write_ = (UINT8_T *)realloc(packetData[port_num].data_write_, 2 * sizeof(UINT8_T));
packetData[port_num].data_write_[0] = DXL_LOBYTE(data);
packetData[port_num].data_write_[1] = DXL_HIBYTE(data);
WriteTxRx1(port_num, id, address, 2);
}
void Dynamixel::pushParam(int value) {
if(mbLengthForPacketMaking > 140)//prevent violation of memory access
return;
mParamBuffer[mbLengthForPacketMaking++] = (unsigned char)DXL_LOBYTE(value);
mParamBuffer[mbLengthForPacketMaking++] = (unsigned char)DXL_HIBYTE(value);
}
void write2ByteTxRx1(int port_num, uint8_t id, uint16_t address, uint16_t data)
{
packetData[port_num].data_write = (uint8_t *)realloc(packetData[port_num].data_write, 2 * sizeof(uint8_t));
packetData[port_num].data_write[0] = DXL_LOBYTE(data);
packetData[port_num].data_write[1] = DXL_HIBYTE(data);
writeTxRx1(port_num, id, address, 2);
}
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;
}
byte Dynamixel::syncWrite(int start_addr, int data_length, word *param, int param_length) {
int i=0, j=0, k=0, num=0;
this->clearBuffer();
num = param_length/(data_length + 1); //ID+DATA1+DATA2..
if(mPacketType == DXL_PACKET_TYPE2) {
mParamBuffer[0] = DXL_LOBYTE(start_addr);
mParamBuffer[1] = DXL_HIBYTE(start_addr);
mParamBuffer[2] = DXL_LOBYTE(data_length*2);
mParamBuffer[3] = DXL_HIBYTE(data_length*2);
for(i=4; i < (4+num*(1+data_length*2)); i+=(1+data_length*2) ) {
mParamBuffer[i] = (byte)param[k++]; //ID
for(j=0; j < (data_length*2); j+=2) {
mParamBuffer[i+j+1] = DXL_LOBYTE(param[k]); //low byte
mParamBuffer[i+j+2] = DXL_HIBYTE(param[k]); //high byte
k++;
}
}
return this->txRxPacket(BROADCAST_ID, INST_SYNC_WRITE, i);
} else {
mbLengthForPacketMaking = 0;
mbIDForPacketMaking = BROADCAST_ID;
mbInstructionForPacketMaking = INST_SYNC_WRITE;
mCommStatus = 0;
mParamBuffer[mbLengthForPacketMaking++] = start_addr;
mParamBuffer[mbLengthForPacketMaking++] = data_length*2;
for(i=mbLengthForPacketMaking; i < num*(1+data_length*2); i+=(1+data_length*2)) {
mParamBuffer[i] = param[k++]; //ID
for(j=0; j < (data_length*2); j+=2) {
mParamBuffer[i+j+1] = DXL_LOBYTE(param[k]); //low byte
mParamBuffer[i+j+2] = DXL_HIBYTE(param[k]);; //high byte
k++;
}
}
mbLengthForPacketMaking= i;
return this->txRxPacket(mbIDForPacketMaking, mbInstructionForPacketMaking, mbLengthForPacketMaking);
}
}
byte Dynamixel::writeWord(byte bID, word bAddress, word wData) {
byte param_length = 0;
this->clearBuffer();
if(mPacketType == DXL_PACKET_TYPE1) {
mParamBuffer[0] = bAddress;
mParamBuffer[1] = DXL_LOBYTE(wData);//(byte)(wData&0xff);
mParamBuffer[2] = DXL_HIBYTE(wData);//(byte)((wData>>8)&0xff);
param_length = 3;
} else {
mParamBuffer[0] = (unsigned char)DXL_LOBYTE(bAddress);
mParamBuffer[1] = (unsigned char)DXL_HIBYTE(bAddress);
//insert data to parameter bucket
mParamBuffer[2] = DXL_LOBYTE(wData);
mParamBuffer[3] = DXL_HIBYTE(wData);
param_length = 4;
}
return this->txRxPacket(bID, INST_WRITE, param_length);
}
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;
}
/*
* @brief Sets the target position and speed of the specified servo
* @author Made by Martin S. Mason(Professor @Mt. San Antonio College)
* @change 2013-04-17 changed by ROBOTIS,.LTD.
* */
byte Dynamixel::setPosition(byte ServoID, int Position, int Speed) {
byte param_length = 0;
if(mPacketType == DXL_PACKET_TYPE1) {
mParamBuffer[0] = (unsigned char)30;
mParamBuffer[1] = (unsigned char)DXL_LOBYTE(Position);
mParamBuffer[2] = (unsigned char)DXL_HIBYTE(Position);
mParamBuffer[3] = (unsigned char)DXL_LOBYTE(Speed);
mParamBuffer[4] = (unsigned char)DXL_HIBYTE(Speed);
param_length = 5;
} else {
mParamBuffer[0] = 30;
mParamBuffer[1] = 0;
//insert data to parameter bucket
mParamBuffer[2] = (unsigned char)DXL_LOBYTE(Position);
mParamBuffer[3] = (unsigned char)DXL_HIBYTE(Position);
mParamBuffer[4] = (unsigned char)DXL_LOBYTE(Speed);
mParamBuffer[5] = (unsigned char)DXL_HIBYTE(Speed);
param_length = 6;
}
return (this->txRxPacket(ServoID, INST_WRITE, param_length));
}
uint32 Dynamixel::readDword( byte bID, word wAddress ) {
if(mPacketType == DXL_PACKET_TYPE1) return 0xFFFFFFFF;
mParamBuffer[0] = (unsigned char)DXL_LOBYTE(wAddress);
mParamBuffer[1] = (unsigned char)DXL_HIBYTE(wAddress);
mParamBuffer[2] = 4; //4byte
mParamBuffer[3] = 0;
if(this->txRxPacket(bID, INST_READ, 4)) {
return DXL_MAKEDWORD( DXL_MAKEWORD( mRxBuffer[9], mRxBuffer[10]),
DXL_MAKEWORD( mRxBuffer[11], mRxBuffer[12]));
} else {
return 0xFFFFFFFF;
}
}
byte Dynamixel::writeByte(byte bID, word bAddress, byte bData) {
byte param_length = 0;
if(mPacketType == DXL_PACKET_TYPE1) {
mParamBuffer[0] = bAddress;
mParamBuffer[1] = bData;
param_length = 2;
} else {
//insert wAddress to parameter bucket
mParamBuffer[0] = (unsigned char)DXL_LOBYTE(bAddress);
mParamBuffer[1] = (unsigned char)DXL_HIBYTE(bAddress);
//insert data to parameter bucket
mParamBuffer[2] = bData;
param_length = 3;
}
return this->txRxPacket(bID, INST_WRITE, param_length);
}
void setDataWrite1(int port_num, uint16_t data_length, uint16_t data_pos, uint32_t data)
{
packetData[port_num].data_write = (uint8_t *)realloc(packetData[port_num].data_write, (data_pos + data_length) * sizeof(uint8_t));
switch (data_length)
{
case 1:
packetData[port_num].data_write[data_pos + 0] = DXL_LOBYTE(DXL_LOWORD(data));
break;
case 2:
packetData[port_num].data_write[data_pos + 0] = DXL_LOBYTE(DXL_LOWORD(data));
packetData[port_num].data_write[data_pos + 1] = DXL_HIBYTE(DXL_LOWORD(data));
break;
default:
printf("[Set Data for Write] failed");
break;
}
}
void SetDataWrite1(int port_num, UINT16_T data_length, UINT16_T data_pos, UINT32_T data)
{
packetData[port_num].data_write_ = (UINT8_T *)realloc(packetData[port_num].data_write_, (data_pos + data_length) * sizeof(UINT8_T));
switch (data_length)
{
case 1:
packetData[port_num].data_write_[data_pos + 0] = DXL_LOBYTE(DXL_LOWORD(data));
break;
case 2:
packetData[port_num].data_write_[data_pos + 0] = DXL_LOBYTE(DXL_LOWORD(data));
packetData[port_num].data_write_[data_pos + 1] = DXL_HIBYTE(DXL_LOWORD(data));
break;
default:
printf("[Set Data for Write] failed");
break;
}
}
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;
}
