diff --git a/src/dynamixel.cpp b/src/dynamixel.cpp
index 7c85ff51a3bde2ff5e0469130f6a9316360d3ce6..a5a88cef27b34d24313a92eaeac92c6d865b6415 100644
--- a/src/dynamixel.cpp
+++ b/src/dynamixel.cpp
@@ -15,35 +15,291 @@ CDynamixel::CDynamixel(std::string& cont_id)
this->node_address=-1;
this->id_register=0x03;
this->baudrate_register=0x04;
+ this->version=dyn_version1;
}
-unsigned char CDynamixel::compute_checksum(unsigned char *packet,int len)
+void CDynamixel::send_instruction_packet_v1(dyn_inst_t inst,unsigned char *data,unsigned char len)
{
- int i=0;
- short int checksum=0;
+ unsigned char *packet;
+ int i,length;
- if(packet==NULL)
+ switch(inst)
{
- /* handle exceptions */
- throw CDynamixelException(_HERE_,"Invalid packet structure",this->node_address);
+ case dyn_reg_write:
+ case dyn_write:
+ case dyn_read: length=6+len;
+ packet=new unsigned char[length];
+ break;
+ default: throw CDynamixelException(_HERE_,"Instruction not supported",this->node_address);
+ break;
}
- else
+ packet[0]=0xFF;
+ packet[1]=0xFF;
+ packet[2]=this->node_address;
+ packet[3]=len+2;
+ packet[4]=inst;
+ for(i=0;i<len;i++)
+ packet[5+i]=data[i];
+ // byte stuffing
+ packet[length-1]=0x00;
+ packet[length-1]=CDynamixelServer::compute_checksum_v1(packet,length);
+ if(this->usb_dev!=NULL)
{
- if(len<=0)
+ this->usb_access->enter();
+ if(this->usb_dev->write(packet,length)!=length)
{
/* handle exceptions */
- throw CDynamixelException(_HERE_,"Invalid packet length",this->node_address);
+ this->usb_access->exit();
+ throw CDynamixelException(_HERE_,"Unexpected error while writing to the communication device",this->node_address);
}
- else
+ this->usb_access->exit();
+ }
+ else
+ {
+ /* handle exceptions */
+ throw CDynamixelException(_HERE_,"The communication device is not properly configured",this->node_address);
+ }
+}
+
+void CDynamixel::send_instruction_packet_v2(dyn_inst_t inst,unsigned char *data,unsigned short int len)
+{
+ unsigned char *packet;
+ int i,length,crc;
+
+ switch(inst)
+ {
+ case dyn_reg_write:
+ case dyn_write:
+ case dyn_read: length=10+len;
+ packet=new unsigned char[length];
+ break;
+ default: throw CDynamixelException(_HERE_,"Instruction not supported",this->node_address);
+ break;
+ }
+ packet[0]=0xFF;
+ packet[1]=0xFF;
+ packet[2]=0xFD;
+ packet[3]=0x00;
+ packet[4]=this->node_address;
+ packet[5]=(len+3)%256;
+ packet[6]=(len+3)/256;
+ packet[7]=inst;
+ for(i=0;i<len;i++)
+ packet[8+i]=data[i];
+ packet[length-2]=0x00;
+ packet[length-1]=0x00;
+ crc=CDynamixelServer::compute_checksum_v2(packet,length-2);
+ packet[length-2]=crc%256;
+ packet[length-1]=crc/256;
+ if(this->usb_dev!=NULL)
+ {
+ this->usb_access->enter();
+ if(this->usb_dev->write(packet,length)!=length)
{
- for(i=2;i<len;i++)
- checksum+=packet[i];
- checksum=~checksum;
- checksum=checksum%256;
+ /* handle exceptions */
+ this->usb_access->exit();
+ throw CDynamixelException(_HERE_,"Unexpected error while writing to the communication device",this->node_address);
}
+ this->usb_access->exit();
}
+ else
+ {
+ /* handle exceptions */
+ throw CDynamixelException(_HERE_,"The communication device is not properly configured",this->node_address);
+ }
+}
- return checksum;
+unsigned char CDynamixel::receive_status_packet_v1(unsigned char **data,unsigned char *len)
+{
+ std::list<std::string> events;
+ unsigned char data_int[256];
+ int num=0,read=0,length;
+
+ if(this->usb_dev!=NULL)
+ {
+ try{
+ this->usb_access->enter();
+ events.push_back(this->usb_dev->get_rx_event_id());
+ // read up to the length field
+ do{
+ if((num=this->usb_dev->get_num_data())==0)
+ {
+ this->event_server->wait_all(events,20);
+ num=this->usb_dev->get_num_data();
+ }
+ if((read+num)>256)
+ {
+ this->usb_dev->read(&data_int[read],256-read);
+ read=256;
+ }
+ else
+ {
+ this->usb_dev->read(&data_int[read],num);
+ read+=num;
+ }
+ }while(read<4);
+ length=data_int[3]+4;
+ // read the remaining of the packet
+ while(read<length)
+ {
+ if((num=this->usb_dev->get_num_data())==0)
+ {
+ this->event_server->wait_all(events,20);
+ num=this->usb_dev->get_num_data();
+ }
+ if((read+num)>256)
+ {
+ this->usb_dev->read(&data_int[read],256-read);
+ read=256;
+ }
+ else
+ {
+ this->usb_dev->read(&data_int[read],num);
+ read+=num;
+ }
+ }
+ // check the checksum
+ if(CDynamixelServer::compute_checksum_v1(data_int,length)!=0x00)
+ {
+ this->usb_access->exit();
+ /* handle exceptions */
+ throw CDynamixelException(_HERE_,"Invalid Checksum",this->node_address);
+ }
+ // byte destuffing
+ // return the error
+ if(length>6)
+ {
+ *data=new unsigned char[length-6];
+ memcpy(*data,&data_int[5],length-6);
+ *len=length-6;
+ }
+ else
+ {
+ *data=NULL;
+ *len=0;
+ }
+ this->usb_access->exit();
+ return data_int[4];
+ }catch(CEventTimeoutException &e){
+ this->usb_access->exit();
+ throw e;
+ }
+ }
+ else
+ {
+ /* handle exceptions */
+ throw CDynamixelException(_HERE_,"The communication device is not properly configured",this->node_address);
+ }
+}
+
+unsigned char CDynamixel::receive_status_packet_v2(unsigned char **data,unsigned short int *len)
+{
+ std::list<std::string> events;
+ unsigned char data_int[256];
+ int num=0,read=0,length;
+ unsigned short int crc;
+
+ if(this->usb_dev!=NULL)
+ {
+ try{
+ this->usb_access->enter();
+ events.push_back(this->usb_dev->get_rx_event_id());
+ // read up to the length field
+ do{
+ if((num=this->usb_dev->get_num_data())==0)
+ {
+ this->event_server->wait_all(events,20);
+ num=this->usb_dev->get_num_data();
+ }
+ if((read+num)>256)
+ {
+ this->usb_dev->read(&data_int[read],256-read);
+ read=256;
+ }
+ else
+ {
+ this->usb_dev->read(&data_int[read],num);
+ read+=num;
+ }
+ }while(read<7);
+ length=data_int[5]+data_int[6]*256+7;
+ // read the remaining of the packet
+ while(read<length)
+ {
+ if((num=this->usb_dev->get_num_data())==0)
+ {
+ this->event_server->wait_all(events,20);
+ num=this->usb_dev->get_num_data();
+ }
+ if((read+num)>256)
+ {
+ this->usb_dev->read(&data_int[read],256-read);
+ read=256;
+ }
+ else
+ {
+ this->usb_dev->read(&data_int[read],num);
+ read+=num;
+ }
+ }
+ // check the checksum
+ crc=CDynamixelServer::compute_checksum_v2(data_int,length-2);
+ if((crc%256)!=data_int[length-2] || (crc/256)!=data_int[length-1])
+ {
+ this->usb_access->exit();
+ /* handle exceptions */
+ throw CDynamixelException(_HERE_,"Invalid Checksum",this->node_address);
+ }
+ // return the error
+ if(length>11)
+ {
+ *data=new unsigned char[length-11];
+ memcpy(*data,&data_int[9],length-11);
+ *len=length-11;
+ }
+ else
+ {
+ *data=NULL;
+ *len=0;
+ }
+ this->usb_access->exit();
+ return data_int[8];
+ }catch(CEventTimeoutException &e){
+ this->usb_access->exit();
+ throw e;
+ }
+ }
+ else
+ {
+ /* handle exceptions */
+ throw CDynamixelException(_HERE_,"The communication device is not properly configured",this->node_address);
+ }
+}
+
+void CDynamixel::handle_error(unsigned char error)
+{
+ std::string error_msg;
+
+ if(error!=0x00)
+ {
+ /* handle exceptions */
+ if(error&0x01)
+ error_msg+="\nThe supply voltage is out of range";
+ if(error&0x02)
+ error_msg+="\nThe goal position is out of range";
+ if(error&0x04)
+ error_msg+="\nThe internal temperature is too high";
+ if(error&0x08)
+ error_msg+="\nThe provided parameter is out of range";
+ if(error&0x10)
+ error_msg+="\nThe packet sent to the dynamixel device had an invalid checksum";
+ if(error&0x20)
+ error_msg+="\nOverload";
+ if(error&0x40)
+ error_msg+="\nInvalid instruction";
+ std::cout << error_msg << std::endl;
+ throw CDynamixelAlarmException(_HERE_,error_msg,this->node_address,error);
+ }
}
void CDynamixel::set_baudrate(int baudrate)
@@ -56,6 +312,7 @@ void CDynamixel::set_baudrate(int baudrate)
else
{
/* handle exceptions */
+ throw CDynamixelException(_HERE_,"The communication device is not properly configured",this->node_address);
}
}
@@ -76,6 +333,7 @@ void CDynamixel::set_id(unsigned char id)
else
{
/* handle exceptions */
+ throw CDynamixelException(_HERE_,"The communication device is not properly configured",id);
}
}
@@ -109,829 +367,296 @@ void CDynamixel::resync(void)
}
}
-void CDynamixel::read_byte_register(unsigned char address,unsigned char *value)
+void CDynamixel::read_byte_register(unsigned short int address,unsigned char *value)
{
- unsigned char packet[8]={0xFF,0xFF,0x00,0x04,0x02,0x00,0x00,0x00};
- int num=0,read=0,it=0,read_length=7,i=0;
- std::list<std::string> events;
- unsigned char data[7],dummy;
- std::string error_msg;
- bool rx_tx_ok=false;
+ unsigned char *data,error,length_v1,cmd[4];
+ unsigned short length_v2;
- if(this->usb_dev!=NULL)
+ if(version==dyn_version1)
{
- events.push_back(this->usb_rx_event_id);
- while(!rx_tx_ok)
- {
- this->usb_access->enter();
- packet[2]=this->node_address;
- packet[5]=(unsigned char)address;
- packet[6]=1;
- packet[7]=this->compute_checksum(packet,8);
-
- if(this->usb_dev->write(packet,8)!=8)
- {
- /* handle exceptions */
- this->usb_access->exit();
- throw CDynamixelException(_HERE_,"Unexpected error while writing to the dynamixel device",this->node_address);
- }
- else
- {
- try{
- while(read<read_length)
- {
- this->event_server->wait_all(events,100);
- num=this->usb_dev->get_num_data();
- if(read+num>read_length)
- {
- this->usb_dev->read(&data[read],read_length-read);
- for(i=0;i<(num-read_length+read);i++)
- this->usb_dev->read(&dummy,1);
- }
- else
- this->usb_dev->read(&data[read],num);
- read+=num;
- if(read>5 && data[4]!=0x00)
- read_length=6;
- }
- if(this->compute_checksum(data,read_length)!=0)
- {
- /* handle exceptions */
- this->usb_access->exit();
- it++;
- if(it==NUM_RETRIES)
- throw CDynamixelSyncException("Invalid packet checksum");
- else
- {
- read=0;
- read_length=7;
- }
- }
- else
- {
- rx_tx_ok=true;// transmission ok
- this->usb_access->exit();
- }
- }catch(CEventTimeoutException &e){
- this->resync();
- this->usb_access->exit();
- }
- }
- }
- if(data[4]!=0x00)
- {
- /* handle exceptions */
- if(data[4]&0x01)
- error_msg+="\nThe supply voltage is out of range";
- if(data[4]&0x02)
- error_msg+="\nThe goal position is out of range";
- if(data[4]&0x04)
- error_msg+="\nThe internal temperature is too high";
- if(data[4]&0x08)
- error_msg+="\nThe provided parameter is out of range";
- if(data[4]&0x10)
- error_msg+="\nThe packet sent to the dynamixel device had an invalid checksum";
- if(data[4]&0x20)
- error_msg+="\nOverload";
- if(data[4]&0x40)
- error_msg+="\nInvalid instruction";
- throw CDynamixelAlarmException(_HERE_,error_msg,this->node_address,data[4]);
- }
- else
- *value=data[5];
+ cmd[0]=address%256;
+ cmd[1]=1;
+ this->send_instruction_packet_v1(dyn_read,cmd,2);
+ error=this->receive_status_packet_v1(&data,&length_v1);
}
else
{
- /* handle exceptions */
- throw CDynamixelException(_HERE_,"The communication device is not ready to send data",this->node_address);
+ cmd[0]=address%256;
+ cmd[1]=address/256;
+ cmd[2]=1;
+ cmd[3]=0;
+ this->send_instruction_packet_v2(dyn_read,cmd,4);
+ error=this->receive_status_packet_v2(&data,&length_v2);
+ }
+ try{
+ this->handle_error(error);
+ *value=data[0];
+ if(data!=NULL)
+ delete[] data;
+ }catch(CException &e){
+ if(data!=NULL)
+ delete[] data;
+ throw e;
}
}
-void CDynamixel::read_word_register(unsigned char address,unsigned short int *value)
+void CDynamixel::read_word_register(unsigned short int address,unsigned short int *value)
{
- unsigned char packet[8]={0xFF,0xFF,0x00,0x04,0x02,0x00,0x00,0x00};
- int num=0,read=0,it=0,read_length=8,i=0;
- std::list<std::string> events;
- unsigned char data[8],dummy;
- std::string error_msg;
- bool rx_tx_ok=false;
+ unsigned char *data,error,length_v1,cmd[4];
+ unsigned short length_v2;
- if(this->usb_dev!=NULL)
+ if(version==dyn_version1)
{
- events.push_back(this->usb_rx_event_id);
- while(!rx_tx_ok)
- {
- this->usb_access->enter();
- packet[2]=this->node_address;
- packet[5]=(unsigned char)address;
- packet[6]=2;
- packet[7]=this->compute_checksum(packet,8);
-
- if(this->usb_dev->write(packet,8)!=8)
- {
- /* handle exceptions */
- this->usb_access->exit();
- throw CDynamixelException(_HERE_,"Unexpected error while writing to the dynamixel device",this->node_address);
- }
- else
- {
- try{
- while(read<read_length)
- {
- this->event_server->wait_all(events,100);
- num=this->usb_dev->get_num_data();
- if(read+num>read_length)
- {
- this->usb_dev->read(&data[read],read_length-read);
- for(i=0;i<(num-read_length+read);i++)
- this->usb_dev->read(&dummy,1);
- }
- else
- this->usb_dev->read(&data[read],num);
- read+=num;
- if(read>5 && data[4]!=0x00)
- read_length=6;
- }
- if(this->compute_checksum(data,read_length)!=0)
- {
- /* handle exceptions */
- this->usb_access->exit();
- it++;
- if(it==NUM_RETRIES)
- throw CDynamixelSyncException("Invalid packet checksum");
- else
- {
- read=0;
- read_length=8;
- }
- }
- else
- {
- rx_tx_ok=true;// transmission ok
- this->usb_access->exit();
- }
- }catch(CEventTimeoutException &e){
- this->resync();
- this->usb_access->exit();
- }
- }
- }
- if(data[4]!=0x00)
- {
- /* handle exceptions */
- if(data[4]&0x01)
- error_msg+="\nThe supply voltage is out of range";
- if(data[4]&0x02)
- error_msg+="\nThe goal position is out of range";
- if(data[4]&0x04)
- error_msg+="\nThe internal temperature is too high";
- if(data[4]&0x08)
- error_msg+="\nThe provided parameter is out of range";
- if(data[4]&0x10)
- error_msg+="\nThe packet sent to the dynamixel device had an invalid checksum";
- if(data[4]&0x20)
- error_msg+="\nOverload";
- if(data[4]&0x40)
- error_msg+="\nInvalid instruction";
- throw CDynamixelAlarmException(_HERE_,error_msg,this->node_address,data[4]);
- }
- else
- *value=data[5]+data[6]*256;
+ cmd[0]=address%256;
+ cmd[1]=2;
+ this->send_instruction_packet_v1(dyn_read,cmd,2);
+ error=this->receive_status_packet_v1(&data,&length_v1);
}
else
{
- /* handle exceptions */
- throw CDynamixelException(_HERE_,"The communication device is not ready to send data",this->node_address);
+ cmd[0]=address%256;
+ cmd[1]=address/256;
+ cmd[2]=2;
+ cmd[3]=0;
+ this->send_instruction_packet_v2(dyn_read,cmd,4);
+ error=this->receive_status_packet_v2(&data,&length_v2);
+ }
+ try{
+ this->handle_error(error);
+ *value=data[0]+data[1]*256;
+ if(data!=NULL)
+ delete[] data;
+ }catch(CException &e){
+ if(data!=NULL)
+ delete[] data;
+ throw e;
}
}
-void CDynamixel::write_byte_register(unsigned char address, unsigned char data)
+void CDynamixel::write_byte_register(unsigned short int address, unsigned char value)
{
- unsigned char packet[8]={0xFF,0xFF,0x00,0x04,0x03,0x00,0x00,0x00};
- std::list<std::string> events;
- unsigned char answer[6],dummy;
- int num=0,read=0,it=0,i=0;
- std::string error_msg;
- bool rx_tx_ok=false;
+ unsigned char *data,error,length_v1,cmd[3];
+ unsigned short length_v2;
- if(this->usb_dev!=NULL)
+ if(version==dyn_version1)
{
- events.push_back(this->usb_rx_event_id);
- while(!rx_tx_ok)
- {
- this->usb_access->enter();
- packet[2]=this->node_address;
- packet[5]=(unsigned char)address;
- packet[6]=data;
- packet[7]=this->compute_checksum(packet,8);
-
- if(this->usb_dev->write(packet,8)!=8)
- {
- /* handle exceptions */
- this->usb_access->exit();
- throw CDynamixelException(_HERE_,"Unexpected error while writing to the dynamixel device",this->node_address);
- }
- else
- {
- try{
- while(read<6)
- {
- this->event_server->wait_all(events,100);
- num=this->usb_dev->get_num_data();
- if(read+num>6)
- {
- this->usb_dev->read(&answer[read],6-read);
- for(i=0;i<(num-6+read);i++)
- this->usb_dev->read(&dummy,1);
- }
- else
- this->usb_dev->read(&answer[read],num);
- read+=num;
- }
- if(this->compute_checksum(answer,6)!=0)
- {
- /* handle exceptions */
- this->usb_access->exit();
- it++;
- if(it==NUM_RETRIES)
- throw CDynamixelSyncException("Invalid packet checksum");
- else
- read=0;
- }
- else
- {
- rx_tx_ok=true;// transmission ok
- this->usb_access->exit();
- }
- }catch(CEventTimeoutException &e){
- this->resync();
- this->usb_access->exit();
- }
- }
- }
- if(answer[4]!=0x00)
- {
- /* handle exceptions */
- if(answer[4]&0x01)
- error_msg+="\nThe supply voltage is out of range";
- if(answer[4]&0x02)
- error_msg+="\nThe goal position is out of range";
- if(answer[4]&0x04)
- error_msg+="\nThe internal temperature is too high";
- if(answer[4]&0x08)
- error_msg+="\nThe provided parameter is out of range";
- if(answer[4]&0x10)
- error_msg+="\nThe packet sent to the dynamixel device had an invalid checksum";
- if(answer[4]&0x20)
- error_msg+="\nOverload";
- if(answer[4]&0x40)
- error_msg+="\nInvalid instruction";
- throw CDynamixelAlarmException(_HERE_,error_msg,this->node_address,answer[4]);
- }
+ cmd[0]=address%256;
+ cmd[1]=value;
+ this->send_instruction_packet_v1(dyn_write,cmd,2);
+ error=this->receive_status_packet_v1(&data,&length_v1);
}
else
{
- /* handle exceptions */
- throw CDynamixelException(_HERE_,"The communication device is not ready to send data",this->node_address);
+ cmd[0]=address%256;
+ cmd[1]=address/256;
+ cmd[2]=value;
+ this->send_instruction_packet_v2(dyn_write,cmd,3);
+ error=this->receive_status_packet_v2(&data,&length_v2);
}
+ if(data!=NULL)
+ delete[] data;
+ this->handle_error(error);
}
-void CDynamixel::write_word_register(unsigned char address, unsigned short int data)
+void CDynamixel::write_word_register(unsigned short int address, unsigned short int value)
{
- unsigned char packet[9]={0xFF,0xFF,0x00,0x05,0x03,0x00,0x00,0x00,0x00};
- std::list<std::string> events;
- unsigned char answer[6],dummy;
- int num=0,read=0,it=0,i=0;
- std::string error_msg;
- bool rx_tx_ok=false;
+ unsigned char *data,error,length_v1,cmd[4];
+ unsigned short length_v2;
- if(this->usb_dev!=NULL)
+ if(version==dyn_version1)
{
- events.push_back(this->usb_rx_event_id);
- while(!rx_tx_ok)
- {
- this->usb_access->enter();
- packet[2]=this->node_address;
- packet[5]=(unsigned char)address;
- packet[6]=((unsigned char *)&data)[0];
- packet[7]=((unsigned char *)&data)[1];
- packet[8]=this->compute_checksum(packet,9);
-
- if(this->usb_dev->write(packet,9)!=9)
- {
- // handle exceptions
- this->usb_access->exit();
- throw CDynamixelException(_HERE_,"Unexpected error while writing to the dynamixel device",this->node_address);
- }
- else
- {
- try{
- while(read<6)
- {
- this->event_server->wait_all(events,100);
- num=this->usb_dev->get_num_data();
- if(read+num>6)
- {
- this->usb_dev->read(&answer[read],6-read);
- for(i=0;i<(num-6+read);i++)
- this->usb_dev->read(&dummy,1);
- }
- else
- this->usb_dev->read(&answer[read],num);
- read+=num;
- }
- if(this->compute_checksum(answer,6)!=0)
- {
- /* handle exceptions */
- this->usb_access->exit();
- it++;
- if(it==NUM_RETRIES)
- throw CDynamixelSyncException("Invalid packet checksum");
- else
- read=0;
- }
- else
- {
- rx_tx_ok=true;
- this->usb_access->exit();
- }
- }catch(CEventTimeoutException &e){
- this->resync();
- this->usb_access->exit();
- }
- }
- }
- if(answer[4]!=0x00)
- {
- /* handle exceptions */
- if(answer[4]&0x01)
- error_msg+="\nThe supply voltage is out of range";
- if(answer[4]&0x02)
- error_msg+="\nThe goal position is out of range";
- if(answer[4]&0x04)
- error_msg+="\nThe internal temperature is too high";
- if(answer[4]&0x08)
- error_msg+="\nThe provided parameter is out of range";
- if(answer[4]&0x10)
- error_msg+="\nThe packet sent to the dynamixel device had an invalid checksum";
- if(answer[4]&0x20)
- error_msg+="\nOverload";
- if(answer[4]&0x40)
- error_msg+="\nInvalid instruction";
- throw CDynamixelAlarmException(_HERE_,error_msg,this->node_address,answer[4]);
- }
+ cmd[0]=address%256;
+ cmd[1]=value%256;
+ cmd[2]=value/256;
+ this->send_instruction_packet_v1(dyn_write,cmd,3);
+ error=this->receive_status_packet_v1(&data,&length_v1);
}
else
{
- /* handle exceptions */
- throw CDynamixelException(_HERE_,"The communication device is not ready to send data",this->node_address);
+ cmd[0]=address%256;
+ cmd[1]=address/256;
+ cmd[2]=value%256;
+ cmd[3]=value/256;
+ this->send_instruction_packet_v2(dyn_write,cmd,4);
+ error=this->receive_status_packet_v2(&data,&length_v2);
}
+ if(data!=NULL)
+ delete[] data;
+ this->handle_error(error);
}
-void CDynamixel::registered_byte_write(unsigned char address, unsigned char data)
+void CDynamixel::registered_byte_write(unsigned short int address, unsigned char value)
{
- unsigned char packet[8]={0xFF,0xFF,0x00,0x04,0x04,0x00,0x00,0x00};
- std::list<std::string> events;
- unsigned char answer[6],dummy;
- int num=0,read=0,it=0,i=0;
- std::string error_msg;
- bool rx_tx_ok=false;
+ unsigned char *data,error,length_v1,cmd[3];
+ unsigned short length_v2;
- if(this->usb_dev!=NULL)
+ if(version==dyn_version1)
{
- events.push_back(this->usb_rx_event_id);
- while(!rx_tx_ok)
- {
- this->usb_access->enter();
- packet[2]=this->node_address;
- packet[5]=(unsigned char)address;
- packet[6]=data;
- packet[7]=this->compute_checksum(packet,8);
-
- if(this->usb_dev->write(packet,8)!=8)
- {
- /* handle exceptions */
- this->usb_access->exit();
- throw CDynamixelException(_HERE_,"Unexpected error while writing to the dynamixel device",this->node_address);
- }
- else
- {
- try{
- while(read<6)
- {
- this->event_server->wait_all(events,100);
- num=this->usb_dev->get_num_data();
- if(read+num>6)
- {
- this->usb_dev->read(&answer[read],6-read);
- for(i=0;i<(num-6+read);i++)
- this->usb_dev->read(&dummy,1);
- }
- else
- this->usb_dev->read(&answer[read],num);
- read+=num;
- }
- if(this->compute_checksum(answer,6)!=0)
- {
- /* handle exceptions */
- this->usb_access->exit();
- it++;
- if(it==NUM_RETRIES)
- throw CDynamixelSyncException("Invalid packet checksum");
- else
- read=0;
- }
- else
- {
- rx_tx_ok=true;
- this->usb_access->exit();
- }
- }catch(CEventTimeoutException &e){
- this->resync();
- this->usb_access->exit();
- }
- }
- }
- if(answer[4]!=0x00)
- {
- /* handle exceptions */
- if(answer[4]&0x01)
- error_msg+="\nThe supply voltage is out of range";
- if(answer[4]&0x02)
- error_msg+="\nThe goal position is out of range";
- if(answer[4]&0x04)
- error_msg+="\nThe internal temperature is too high";
- if(answer[4]&0x08)
- error_msg+="\nThe provided parameter is out of range";
- if(answer[4]&0x10)
- error_msg+="\nThe packet sent to the dynamixel device had an invalid checksum";
- if(answer[4]&0x20)
- error_msg+="\nOverload";
- if(answer[4]&0x40)
- error_msg+="\nInvalid instruction";
- throw CDynamixelAlarmException(_HERE_,error_msg,this->node_address,answer[4]);
- }
+ cmd[0]=address%256;
+ cmd[1]=value;
+ this->send_instruction_packet_v1(dyn_reg_write,cmd,2);
+ error=this->receive_status_packet_v1(&data,&length_v1);
}
else
{
- /* handle exceptions */
- throw CDynamixelException(_HERE_,"The communication device is not ready to send data",this->node_address);
+ cmd[0]=address%256;
+ cmd[1]=address/256;
+ cmd[2]=value;
+ this->send_instruction_packet_v2(dyn_reg_write,cmd,3);
+ error=this->receive_status_packet_v2(&data,&length_v2);
}
+ if(data!=NULL)
+ delete[] data;
+ this->handle_error(error);
}
-void CDynamixel::registered_word_write(unsigned char address, unsigned short int data)
+void CDynamixel::registered_word_write(unsigned short int address, unsigned short int value)
{
- unsigned char packet[9]={0xFF,0xFF,0x00,0x05,0x04,0x00,0x00,0x00,0x00};
- std::list<std::string> events;
- unsigned char answer[6],dummy;
- int num=0,read=0,it=0,i=0;
- std::string error_msg;
- bool rx_tx_ok=false;
+ unsigned char *data,error,length_v1,cmd[4];
+ unsigned short length_v2;
- if(this->usb_dev!=NULL)
+ if(version==dyn_version1)
{
- events.push_back(this->usb_rx_event_id);
- while(!rx_tx_ok)
- {
- this->usb_access->enter();
- packet[2]=this->node_address;
- packet[5]=(unsigned char)address;
- packet[6]=((unsigned char *)&data)[0];
- packet[7]=((unsigned char *)&data)[1];
- packet[8]=this->compute_checksum(packet,9);
-
- if(this->usb_dev->write(packet,9)!=9)
- {
- // handle exceptions
- this->usb_access->exit();
- throw CDynamixelException(_HERE_,"Unexpected error while writing to the dynamixel device",this->node_address);
- }
- else
- {
- try{
- while(read<6)
- {
- this->event_server->wait_all(events,100);
- num=this->usb_dev->get_num_data();
- if(read+num>6)
- {
- this->usb_dev->read(&answer[read],6-read);
- for(i=0;i<(num-6+read);i++)
- this->usb_dev->read(&dummy,1);
- }
- else
- this->usb_dev->read(&answer[read],num);
- read+=num;
- }
- if(this->compute_checksum(answer,6)!=0)
- {
- /* handle exceptions */
- this->usb_access->exit();
- it++;
- if(it==NUM_RETRIES)
- throw CDynamixelSyncException("Invalid packet checksum");
- else
- read=0;
- }
- else
- {
- rx_tx_ok=true;
- this->usb_access->exit();
- }
- }catch(CEventTimeoutException &e){
- this->resync();
- this->usb_access->exit();
- }
- }
- }
- if(answer[4]!=0x00)
- {
- /* handle exceptions */
- if(answer[4]&0x01)
- error_msg+="\nThe supply voltage is out of range";
- if(answer[4]&0x02)
- error_msg+="\nThe goal position is out of range";
- if(answer[4]&0x04)
- error_msg+="\nThe internal temperature is too high";
- if(answer[4]&0x08)
- error_msg+="\nThe provided parameter is out of range";
- if(answer[4]&0x10)
- error_msg+="\nThe packet sent to the dynamixel device had an invalid checksum";
- if(answer[4]&0x20)
- error_msg+="\nOverload";
- if(answer[4]&0x40)
- error_msg+="\nInvalid instruction";
- throw CDynamixelAlarmException(_HERE_,error_msg,this->node_address,answer[4]);
- }
+ cmd[0]=address%256;
+ cmd[1]=value%256;
+ cmd[2]=value/256;
+ this->send_instruction_packet_v1(dyn_reg_write,cmd,3);
+ error=this->receive_status_packet_v1(&data,&length_v1);
}
else
{
- /* handle exceptions */
- throw CDynamixelException(_HERE_,"The communication device is not ready to send data",this->node_address);
+ cmd[0]=address%256;
+ cmd[1]=address/256;
+ cmd[2]=value%256;
+ cmd[3]=value/256;
+ this->send_instruction_packet_v2(dyn_reg_write,cmd,4);
+ error=this->receive_status_packet_v2(&data,&length_v2);
}
+ if(data!=NULL)
+ delete[] data;
+ this->handle_error(error);
}
-void CDynamixel::write_registers(unsigned char address, unsigned char *data, unsigned int length)
+void CDynamixel::registered_write(unsigned short int address, unsigned char *values,unsigned int length)
{
- unsigned char *packet;
- std::list<std::string> events;
- unsigned char answer[6],dummy;
- int num=0,read=0,it=0;
- std::string error_msg;
- bool rx_tx_ok=false;
- unsigned int i=0;
+ unsigned char *data,error,length_v1,*cmd;
+ unsigned short length_v2,i;
- if(this->usb_dev!=NULL)
+ if(version==dyn_version1)
{
- if(address+length>255)
- {
- /* handle exceptions */
- throw CDynamixelException(_HERE_,"Invalid address range",this->node_address);
- }
- else
- {
- events.push_back(this->usb_rx_event_id);
- while(!rx_tx_ok)
- {
- packet=new unsigned char[length+7];
- this->usb_access->enter();
- packet[0]=0xFF;
- packet[1]=0xFF;
- packet[2]=this->node_address;
- packet[3]=length+3;
- packet[4]=0x03;
- packet[5]=(unsigned char)address;
- for(i=0;i<length;i++)
- packet[i+6]=data[i];
- packet[length+6]=this->compute_checksum(packet,length+6);
-
- if(this->usb_dev->write(packet,length+7)!=(length+7))
- {
- /* handle exceptions */
- this->usb_access->exit();
- delete[] packet;
- throw CDynamixelException(_HERE_,"Unexpected error while writing to the dynamixel device",this->node_address);
- }
- else
- {
- delete[] packet;
- try{
- while(read<6)
- {
- this->event_server->wait_all(events,100);
- num=this->usb_dev->get_num_data();
- if(read+num>6)
- {
- this->usb_dev->read(&answer[read],6-read);
- for(i=0;i<(num-6+read);i++)
- this->usb_dev->read(&dummy,1);
- }
- else
- this->usb_dev->read(&answer[read],num);
- read+=num;
- }
- if(this->compute_checksum(answer,6)!=0)
- {
- /* handle exceptions */
- this->usb_access->exit();
- it++;
- if(it==NUM_RETRIES)
- throw CDynamixelSyncException("Invalid packet checksum");
- else
- read=0;
- }
- else
- {
- rx_tx_ok=true;// transmission ok
- this->usb_access->exit();
- }
- }catch(CEventTimeoutException &e){
- this->resync();
- this->usb_access->exit();
- }
- }
- }
- if(answer[4]!=0x00)
- {
- /* handle exceptions */
- if(answer[4]&0x01)
- error_msg+="\nThe supply voltage is out of range";
- if(answer[4]&0x02)
- error_msg+="\nThe goal position is out of range";
- if(answer[4]&0x04)
- error_msg+="\nThe internal temperature is too high";
- if(answer[4]&0x08)
- error_msg+="\nThe provided parameter is out of range";
- if(answer[4]&0x10)
- error_msg+="\nThe packet sent to the dynamixel device had an invalid checksum";
- if(answer[4]&0x20)
- error_msg+="\nOverload";
- if(answer[4]&0x40)
- error_msg+="\nInvalid instruction";
- throw CDynamixelAlarmException(_HERE_,error_msg,this->node_address,answer[4]);
- }
- }
+ cmd=new unsigned char[length+1];
+ cmd[0]=address%256;
+ for(i=0;i<length;i++)
+ cmd[1+i]=values[i];
+ this->send_instruction_packet_v1(dyn_reg_write,cmd,length+1);
+ delete[] cmd;
+ error=this->receive_status_packet_v1(&data,&length_v1);
}
else
{
- /* handle exceptions */
- throw CDynamixelException(_HERE_,"The communication device is not ready to send data",this->node_address);
+ cmd=new unsigned char[length+2];
+ cmd[0]=address%256;
+ cmd[1]=address/256;
+ for(i=0;i<length;i++)
+ cmd[2+i]=values[i];
+ this->send_instruction_packet_v2(dyn_reg_write,cmd,length+2);
+ delete[] cmd;
+ error=this->receive_status_packet_v2(&data,&length_v2);
}
+ if(data!=NULL)
+ delete[] data;
+ this->handle_error(error);
}
-void CDynamixel::read_registers(unsigned char address, unsigned char *data, unsigned int length)
+void CDynamixel::write_registers(unsigned short int address, unsigned char *values, unsigned int length)
{
- unsigned char packet[8]={0xFF,0xFF,0x00,0x04,0x02,0x00,0x00,0x00};
- int num=0,read=0,it=0,read_length=length+6;
- std::list<std::string> events;
- unsigned char *answer,dummy;
- std::string error_msg;
- bool rx_tx_ok=false;
- unsigned int i=0;
+ unsigned char *data,error,length_v1,*cmd;
+ unsigned short length_v2,i;
- if(this->usb_dev!=NULL)
+ if(version==dyn_version1)
{
- events.push_back(this->usb_rx_event_id);
- while(!rx_tx_ok)
- {
- this->usb_access->enter();
- packet[2]=this->node_address;
- packet[5]=(unsigned char)address;
- packet[6]=length;
- packet[7]=this->compute_checksum(packet,8);
-
- if(this->usb_dev->write(packet,8)!=8)
- {
- /* handle exceptions */
- this->usb_access->exit();
- throw CDynamixelException(_HERE_,"Unexpected error while writing to the dynamixel device",this->node_address);
- }
- else
- {
- try{
- answer=new unsigned char[length+6];
- while(read<read_length)
- {
- this->event_server->wait_all(events,100);
- num=this->usb_dev->get_num_data();
- if(read+num>read_length)
- {
- this->usb_dev->read(&answer[read],read_length-read);
- for(i=0;i<(num-read_length+read);i++)
- this->usb_dev->read(&dummy,1);
- }
- else
- this->usb_dev->read(&answer[read],num);
- read+=num;
- if(read>5 && answer[4]!=0x00)
- read_length=6;
- }
- if(this->compute_checksum(answer,read_length)!=0)
- {
- /* handle exceptions */
- this->usb_access->exit();
- it++;
- if(it==NUM_RETRIES)
- {
- delete[] answer;
- throw CDynamixelSyncException("Invalid packet checksum");
- }
- else
- {
- read=0;
- read_length=length+6;
- }
- }
- else
- {
- rx_tx_ok=true;// transmission ok
- this->usb_access->exit();
- }
- }catch(CEventTimeoutException &e){
- this->resync();
- this->usb_access->exit();
- }
- }
- }
- if(answer[4]!=0x00)
- {
- /* handle exceptions */
- if(answer[4]&0x01)
- error_msg+="\nThe supply voltage is out of range";
- if(answer[4]&0x02)
- error_msg+="\nThe goal position is out of range";
- if(answer[4]&0x04)
- error_msg+="\nThe internal temperature is too high";
- if(answer[4]&0x08)
- error_msg+="\nThe provided parameter is out of range";
- if(answer[4]&0x10)
- error_msg+="\nThe packet sent to the dynamixel device had an invalid checksum";
- if(answer[4]&0x20)
- error_msg+="\nOverload";
- if(answer[4]&0x40)
- error_msg+="\nInvalid instruction";
- delete[] answer;
- throw CDynamixelAlarmException(_HERE_,error_msg,this->node_address,answer[4]);
- }
- else
- {
- for(i=0;i<length;i++)
- data[i]=answer[i+5];
- delete[] answer;
- }
+ cmd=new unsigned char[length+1];
+ cmd[0]=address%256;
+ for(i=0;i<length;i++)
+ cmd[1+i]=values[i];
+ this->send_instruction_packet_v1(dyn_write,cmd,length+1);
+ delete[] cmd;
+ error=this->receive_status_packet_v1(&data,&length_v1);
}
else
{
- /* handle exceptions */
- throw CDynamixelException(_HERE_,"The communication device is not ready to send data",this->node_address);
+ cmd=new unsigned char[length+2];
+ cmd[0]=address%256;
+ cmd[1]=address/256;
+ for(i=0;i<length;i++)
+ cmd[2+i]=values[i];
+ this->send_instruction_packet_v2(dyn_write,cmd,length+2);
+ delete[] cmd;
+ error=this->receive_status_packet_v2(&data,&length_v2);
}
+ if(data!=NULL)
+ delete[] data;
+ this->handle_error(error);
}
-void CDynamixel::reset(void)
+void CDynamixel::read_registers(unsigned short int address, unsigned char *values, unsigned int length)
{
- unsigned char packet[6]={0xFF,0xFF,0x00,0x02,0x06,0x00};
- std::list<std::string> events;
- unsigned char data[6];
- int num=0,read=0;
+ unsigned char *data,error,length_v1,cmd[4];
+ unsigned short length_v2,i;
- if(this->usb_dev!=NULL)
+ if(version==dyn_version1)
{
- this->usb_access->enter();
- packet[2]=this->node_address;
- packet[5]=this->compute_checksum(packet,6);
+ cmd[0]=address%256;
+ cmd[1]=length%256;
+ this->send_instruction_packet_v1(dyn_read,cmd,2);
+ error=this->receive_status_packet_v1(&data,&length_v1);
+ }
+ else
+ {
+ cmd[0]=address%256;
+ cmd[1]=address/256;
+ cmd[2]=length%256;
+ cmd[3]=length/256;
+ this->send_instruction_packet_v2(dyn_read,cmd,4);
+ error=this->receive_status_packet_v2(&data,&length_v2);
+ }
+ try{
+ this->handle_error(error);
+ for(i=0;i<length;i++)
+ values[i]=data[i];
+ if(data!=NULL)
+ delete[] data;
+ }catch(CException &e){
+ if(data!=NULL)
+ delete[] data;
+ throw e;
+ }
+}
- events.push_back(this->usb_rx_event_id);
-
- if(this->usb_dev->write(packet,6)!=6)
- {
- // handle exceptions
- this->usb_access->exit();
- throw CDynamixelException(_HERE_,"Unexpected error while writing to the dynamixel device",this->node_address);
- }
- else
- {
- try{
- while(read<6)
- {
- this->event_server->wait_all(events,100);
- num=this->usb_dev->get_num_data();
- if(num>(6-read))
- this->usb_dev->read(&data[read],6-read);
- else
- this->usb_dev->read(&data[read],num);
- read+=num;
- }
- }catch(CEventTimeoutException &e){
- this->usb_access->exit();
- throw;
- }
- }
- this->usb_access->exit();
+void CDynamixel::reset(dyn_reset_mode_t mode)
+{
+ unsigned char *data,error,length_v1,cmd;
+ unsigned short length_v2;
+
+ if(version==dyn_version1)
+ {
+ this->send_instruction_packet_v1(dyn_read,NULL,0);
+ error=this->receive_status_packet_v1(&data,&length_v1);
}
else
{
- /* handle exceptions */
- throw CDynamixelException(_HERE_,"The communication device is not ready to send data",this->node_address);
+ cmd=mode;
+ this->send_instruction_packet_v2(dyn_read,&cmd,1);
+ error=this->receive_status_packet_v2(&data,&length_v2);
+ }
+ try{
+ this->handle_error(error);
+ if(data!=NULL)
+ delete[] data;
+ }catch(CException &e){
+ if(data!=NULL)
+ delete[] data;
+ throw e;
}
}
diff --git a/src/dynamixel.h b/src/dynamixel.h
index c38753e399b176876d2c78b8deeee76538c4fefa..682879c3775d6d28b1df4b354f327d7f6127f93b 100644
--- a/src/dynamixel.h
+++ b/src/dynamixel.h
@@ -12,6 +12,23 @@
const int NUM_RETRIES=10;
+typedef enum {dyn_version1,dyn_version2} dyn_version_t;
+
+typedef enum {dyn_ping=0x01,
+ dyn_read=0x02,
+ dyn_write=0x03,
+ dyn_reg_write=0x04,
+ dyn_action=0x05,
+ dyn_factory_reset=0x06,
+ dyn_reboot=0x08,
+ dyn_status=0x55,
+ dyn_sync_read=0x82,
+ dyn_sync_write=0x83,
+ dyn_bulk_read=0x92,
+ dyn_bulk_write=0x93} dyn_inst_t;
+
+typedef enum {dyn_reset_all=0xFF,dyn_reset_keep_id=0x01,dyn_reset_keep_id_baud=0x02} dyn_reset_mode_t;
+
/**
* \brief
*
@@ -55,11 +72,6 @@ class CDynamixel
*
*/
unsigned char baudrate_register;
- /**
- * \brief Function to compute the checksum of the data packets
- *
- */
- unsigned char compute_checksum(unsigned char *packet,int len);
/**
* \brief
*
@@ -70,6 +82,37 @@ class CDynamixel
*
*/
void resync(void);
+ /**
+ * \brief
+ *
+ */
+ dyn_version_t version;
+ protected:
+ /**
+ * \brief
+ *
+ */
+ void send_instruction_packet_v1(dyn_inst_t inst,unsigned char *data=NULL,unsigned char len=0);
+ /**
+ * \brief
+ *
+ */
+ void send_instruction_packet_v2(dyn_inst_t inst,unsigned char *data=NULL,unsigned short int len=0);
+ /**
+ * \brief
+ *
+ */
+ unsigned char receive_status_packet_v1(unsigned char **data,unsigned char *len);
+ /**
+ * \brief
+ *
+ */
+ unsigned char receive_status_packet_v2(unsigned char **data,unsigned short int *len);
+ /**
+ * \brief
+ *
+ */
+ void handle_error(unsigned char error);
public:
/**
* \brief
@@ -80,47 +123,52 @@ class CDynamixel
* \brief Function to read a register
*
*/
- void read_byte_register(unsigned char address, unsigned char *value);
+ void read_byte_register(unsigned short int address, unsigned char *value);
/**
* \brief Function to read a register
*
*/
- void read_word_register(unsigned char address,unsigned short int *value);
+ void read_word_register(unsigned short int address,unsigned short int *value);
+ /**
+ * \brief Function to write to a register
+ *
+ */
+ void write_byte_register(unsigned short int address, unsigned char value);
/**
* \brief Function to write to a register
*
*/
- void write_byte_register(unsigned char address, unsigned char data);
+ void write_word_register(unsigned short int address, unsigned short int value);
/**
* \brief Function to write to a register
*
*/
- void write_word_register(unsigned char address, unsigned short int data);
+ void registered_byte_write(unsigned short int address, unsigned char value);
/**
* \brief Function to write to a register
*
*/
- void registered_byte_write(unsigned char address, unsigned char data);
+ void registered_word_write(unsigned short int address, unsigned short int value);
/**
* \brief Function to write to a register
*
*/
- void registered_word_write(unsigned char address, unsigned short int data);
+ void registered_write(unsigned short int address, unsigned char *values,unsigned int length);
/**
* \brief Function to write to a register
*
*/
- void write_registers(unsigned char address, unsigned char *data, unsigned int length);
+ void write_registers(unsigned short int address, unsigned char *values, unsigned int length);
/**
* \brief Function to write to a register
*
*/
- void read_registers(unsigned char address, unsigned char *data, unsigned int length);
+ void read_registers(unsigned short int address, unsigned char *values, unsigned int length);
/**
* \brief
*
*/
- void reset(void);
+ void reset(dyn_reset_mode_t mode=dyn_reset_all);
/**
* \brief
*
diff --git a/src/dynamixelserver.cpp b/src/dynamixelserver.cpp
index dfea759629db4d2296396b9186fac726b4dcd6ef..94839ebcec0f67ea9e0ff4669a618a0fc194b02e 100644
--- a/src/dynamixelserver.cpp
+++ b/src/dynamixelserver.cpp
@@ -4,6 +4,41 @@
#include "ftdiserver.h"
#include <sstream>
+const unsigned short crc_table[256] = {
+ 0x0000, 0x8005, 0x800F, 0x000A, 0x801B, 0x001E, 0x0014, 0x8011,
+ 0x8033, 0x0036, 0x003C, 0x8039, 0x0028, 0x802D, 0x8027, 0x0022,
+ 0x8063, 0x0066, 0x006C, 0x8069, 0x0078, 0x807D, 0x8077, 0x0072,
+ 0x0050, 0x8055, 0x805F, 0x005A, 0x804B, 0x004E, 0x0044, 0x8041,
+ 0x80C3, 0x00C6, 0x00CC, 0x80C9, 0x00D8, 0x80DD, 0x80D7, 0x00D2,
+ 0x00F0, 0x80F5, 0x80FF, 0x00FA, 0x80EB, 0x00EE, 0x00E4, 0x80E1,
+ 0x00A0, 0x80A5, 0x80AF, 0x00AA, 0x80BB, 0x00BE, 0x00B4, 0x80B1,
+ 0x8093, 0x0096, 0x009C, 0x8099, 0x0088, 0x808D, 0x8087, 0x0082,
+ 0x8183, 0x0186, 0x018C, 0x8189, 0x0198, 0x819D, 0x8197, 0x0192,
+ 0x01B0, 0x81B5, 0x81BF, 0x01BA, 0x81AB, 0x01AE, 0x01A4, 0x81A1,
+ 0x01E0, 0x81E5, 0x81EF, 0x01EA, 0x81FB, 0x01FE, 0x01F4, 0x81F1,
+ 0x81D3, 0x01D6, 0x01DC, 0x81D9, 0x01C8, 0x81CD, 0x81C7, 0x01C2,
+ 0x0140, 0x8145, 0x814F, 0x014A, 0x815B, 0x015E, 0x0154, 0x8151,
+ 0x8173, 0x0176, 0x017C, 0x8179, 0x0168, 0x816D, 0x8167, 0x0162,
+ 0x8123, 0x0126, 0x012C, 0x8129, 0x0138, 0x813D, 0x8137, 0x0132,
+ 0x0110, 0x8115, 0x811F, 0x011A, 0x810B, 0x010E, 0x0104, 0x8101,
+ 0x8303, 0x0306, 0x030C, 0x8309, 0x0318, 0x831D, 0x8317, 0x0312,
+ 0x0330, 0x8335, 0x833F, 0x033A, 0x832B, 0x032E, 0x0324, 0x8321,
+ 0x0360, 0x8365, 0x836F, 0x036A, 0x837B, 0x037E, 0x0374, 0x8371,
+ 0x8353, 0x0356, 0x035C, 0x8359, 0x0348, 0x834D, 0x8347, 0x0342,
+ 0x03C0, 0x83C5, 0x83CF, 0x03CA, 0x83DB, 0x03DE, 0x03D4, 0x83D1,
+ 0x83F3, 0x03F6, 0x03FC, 0x83F9, 0x03E8, 0x83ED, 0x83E7, 0x03E2,
+ 0x83A3, 0x03A6, 0x03AC, 0x83A9, 0x03B8, 0x83BD, 0x83B7, 0x03B2,
+ 0x0390, 0x8395, 0x839F, 0x039A, 0x838B, 0x038E, 0x0384, 0x8381,
+ 0x0280, 0x8285, 0x828F, 0x028A, 0x829B, 0x029E, 0x0294, 0x8291,
+ 0x82B3, 0x02B6, 0x02BC, 0x82B9, 0x02A8, 0x82AD, 0x82A7, 0x02A2,
+ 0x82E3, 0x02E6, 0x02EC, 0x82E9, 0x02F8, 0x82FD, 0x82F7, 0x02F2,
+ 0x02D0, 0x82D5, 0x82DF, 0x02DA, 0x82CB, 0x02CE, 0x02C4, 0x82C1,
+ 0x8243, 0x0246, 0x024C, 0x8249, 0x0258, 0x825D, 0x8257, 0x0252,
+ 0x0270, 0x8275, 0x827F, 0x027A, 0x826B, 0x026E, 0x0264, 0x8261,
+ 0x0220, 0x8225, 0x822F, 0x022A, 0x823B, 0x023E, 0x0234, 0x8231,
+ 0x8213, 0x0216, 0x021C, 0x8219, 0x0208, 0x820D, 0x8207, 0x0202
+ };
+
CDynamixelServer *CDynamixelServer::pinstance=NULL;
CDynamixelServer::CDynamixelServer()
@@ -13,16 +48,21 @@ CDynamixelServer::CDynamixelServer()
this->event_server->create_event(this->stop_scan_event_id);
this->scan_done_event_id="dynamixel_server_scan_done_event";
this->event_server->create_event(this->scan_done_event_id);
+ this->scan_error_event_id="dynamixel_server_scan_error_event";
+ this->event_server->create_event(this->scan_error_event_id);
this->thread_server=CThreadServer::instance();
this->scan_thread_id="dynamixel_server_scan_thread";
this->thread_server->create_thread(this->scan_thread_id);
this->thread_server->attach_thread(this->scan_thread_id,this->scan_thread,this);
this->state=dyn_created;
- this->devices.clear();
+ this->devices_v1.clear();
+ this->devices_v2.clear();
this->comm_dev=NULL;
this->bus_info.baud_rate=-1;
this->bus_info.bus_id=-1;
this->bus_info.serial="";
+ this->scan_version=dyn_version1;
+ this->scan_error="No error";
}
CDynamixelServer::CDynamixelServer(const CDynamixelServer& object)
@@ -44,7 +84,275 @@ CDynamixelServer *CDynamixelServer::instance(void)
return CDynamixelServer::pinstance; // Retornamos la dirección de la instancia
}
-unsigned char CDynamixelServer::compute_checksum(unsigned char *packet,int len)
+void CDynamixelServer::send_instruction_packet_v1(dyn_inst_t inst,unsigned char *data,unsigned char len,unsigned char id)
+{
+ unsigned char *packet;
+ int i,length;
+
+ switch(inst)
+ {
+ case dyn_action: id=0xFE;// set the broadcast address
+ case dyn_ping: length=6;
+ packet=new unsigned char[length];
+ if(len!=0)
+ throw CDynamixelServerException(_HERE_,"Invalid data length");
+ break;
+ case dyn_sync_write: id=0xFE;
+ length=6+len;
+ data=new unsigned char[length];
+ break;
+ default: throw CDynamixelServerException(_HERE_,"Instruction not supported");
+ break;
+ }
+ packet[0]=0xFF;
+ packet[1]=0xFF;
+ packet[2]=id;
+ packet[3]=len+2;
+ packet[4]=inst;
+ for(i=0;i<len;i++)
+ packet[5+i]=data[i];
+ // byte stuffing
+ packet[length-1]=0x00;
+ packet[length-1]=this->compute_checksum_v1(packet,length);
+ if(this->comm_dev!=NULL)
+ {
+ this->dynamixel_access.enter();
+ if(this->comm_dev->write(packet,length)!=length)
+ {
+ /* handle exceptions */
+ this->dynamixel_access.exit();
+ throw CDynamixelServerException(_HERE_,"Unexpected error while writing to the communication device");
+ }
+ this->dynamixel_access.exit();
+ }
+ else
+ {
+ /* handle exceptions */
+ throw CDynamixelServerException(_HERE_,"The communication device is not properly configured");
+ }
+}
+
+void CDynamixelServer::send_instruction_packet_v2(dyn_inst_t inst,unsigned char *data,unsigned short int len,unsigned char id)
+{
+ unsigned char *packet;
+ int i,length,crc;
+
+ switch(inst)
+ {
+ case dyn_action: id=0xFE;// set the broadcast address
+ case dyn_ping: length=10;
+ packet=new unsigned char[length];
+ if(len!=0)
+ throw CDynamixelServerException(_HERE_,"Invalid data length");
+ break;
+ case dyn_sync_write: id=0xFE;
+ length=10+len;
+ data=new unsigned char[length];
+ break;
+ default: throw CDynamixelServerException(_HERE_,"Instruction not supported");
+ break;
+ }
+ packet[0]=0xFF;
+ packet[1]=0xFF;
+ packet[2]=0xFD;
+ packet[3]=0x00;
+ packet[4]=id;
+ packet[5]=(len+3)%256;
+ packet[6]=(len+3)/256;
+ packet[7]=inst;
+ for(i=0;i<len;i++)
+ packet[8+i]=data[i];
+ packet[length-2]=0x00;
+ packet[length-1]=0x00;
+ crc=this->compute_checksum_v2(packet,length-2);
+ packet[length-2]=crc%256;
+ packet[length-1]=crc/256;
+ if(this->comm_dev!=NULL)
+ {
+ this->dynamixel_access.enter();
+ if(this->comm_dev->write(packet,length)!=length)
+ {
+ /* handle exceptions */
+ this->dynamixel_access.exit();
+ throw CDynamixelServerException(_HERE_,"Unexpected error while writing to the communication device");
+ }
+ this->dynamixel_access.exit();
+ }
+ else
+ {
+ /* handle exceptions */
+ throw CDynamixelServerException(_HERE_,"The communication device is not properly configured");
+ }
+}
+
+unsigned char CDynamixelServer::receive_status_packet_v1(unsigned char **data,unsigned char *len)
+{
+ std::list<std::string> events;
+ unsigned char data_int[256];
+ int num=0,read=0,length;
+
+ if(this->comm_dev!=NULL)
+ {
+ try{
+ this->dynamixel_access.enter();
+ events.push_back(this->comm_dev->get_rx_event_id());
+ // read up to the length field
+ do{
+ if((num=this->comm_dev->get_num_data())==0)
+ {
+ this->event_server->wait_all(events,20);
+ num=this->comm_dev->get_num_data();
+ }
+ if((read+num)>4)
+ {
+ this->comm_dev->read(&data_int[read],4-read);
+ read=4;
+ }
+ else
+ {
+ this->comm_dev->read(&data_int[read],num);
+ read+=num;
+ }
+ }while(read<4);
+ length=data_int[3]+4;
+ // read the remaining of the packet
+ while(read<length)
+ {
+ if((num=this->comm_dev->get_num_data())==0)
+ {
+ this->event_server->wait_all(events,20);
+ num=this->comm_dev->get_num_data();
+ }
+ if((read+num)>length)
+ {
+ this->comm_dev->read(&data_int[read],length-read);
+ read=length;
+ }
+ else
+ {
+ this->comm_dev->read(&data_int[read],num);
+ read+=num;
+ }
+ }
+ // check the checksum
+ if(this->compute_checksum_v1(data_int,length)!=0x00)
+ {
+ this->dynamixel_access.exit();
+ /* handle exceptions */
+ throw CDynamixelServerException(_HERE_,"Invalid Checksum");
+ }
+ // byte destuffing
+ // return the error
+ if(length>6)
+ {
+ *data=new unsigned char[length-6];
+ memcpy(*data,&data_int[5],length-6);
+ *len=length-6;
+ }
+ else
+ {
+ *data=NULL;
+ *len=0;
+ }
+ this->dynamixel_access.exit();
+ return data_int[4];
+ }catch(CEventTimeoutException &e){
+ this->dynamixel_access.exit();
+ throw e;
+ }
+ }
+ else
+ {
+ /* handle exceptions */
+ throw CDynamixelServerException(_HERE_,"The communication device is not properly configured");
+ }
+}
+
+unsigned char CDynamixelServer::receive_status_packet_v2(unsigned char **data,unsigned short int *len)
+{
+ std::list<std::string> events;
+ unsigned char data_int[256];
+ int num=0,read=0,length;
+ unsigned short int crc;
+
+ if(this->comm_dev!=NULL)
+ {
+ try{
+ this->dynamixel_access.enter();
+ events.push_back(this->comm_dev->get_rx_event_id());
+ // read up to the length field
+ do{
+ if((num=this->comm_dev->get_num_data())==0)
+ {
+ this->event_server->wait_all(events,20);
+ num=this->comm_dev->get_num_data();
+ }
+ if((read+num)>7)
+ {
+ this->comm_dev->read(&data_int[read],7-read);
+ read=7;
+ }
+ else
+ {
+ this->comm_dev->read(&data_int[read],num);
+ read+=num;
+ }
+ }while(read<7);
+ length=data_int[5]+data_int[6]*256+7;
+ // read the remaining of the packet
+ while(read<length)
+ {
+ if((num=this->comm_dev->get_num_data())==0)
+ {
+ this->event_server->wait_all(events,20);
+ num=this->comm_dev->get_num_data();
+ }
+ if((read+num)>length)
+ {
+ this->comm_dev->read(&data_int[read],length-read);
+ read=length;
+ }
+ else
+ {
+ this->comm_dev->read(&data_int[read],num);
+ read+=num;
+ }
+ }
+ // check the checksum
+ crc=this->compute_checksum_v2(data_int,length-2);
+ if((crc%256)!=data_int[length-2] || (crc/256)!=data_int[length-1])
+ {
+ this->dynamixel_access.exit();
+ /* handle exceptions */
+ throw CDynamixelServerException(_HERE_,"Invalid Checksum");
+ }
+ // return the error
+ if(length>11)
+ {
+ *data=new unsigned char[length-11];
+ memcpy(*data,&data_int[9],length-11);
+ *len=length-11;
+ }
+ else
+ {
+ *data=NULL;
+ *len=0;
+ }
+ this->dynamixel_access.exit();
+ return data_int[8];
+ }catch(CEventTimeoutException &e){
+ this->dynamixel_access.exit();
+ throw e;
+ }
+ }
+ else
+ {
+ /* handle exceptions */
+ throw CDynamixelServerException(_HERE_,"The communication device is not properly configured");
+ }
+}
+
+unsigned char CDynamixelServer::compute_checksum_v1(unsigned char *packet,int len)
{
int i=0;
short int checksum=0;
@@ -73,6 +381,111 @@ unsigned char CDynamixelServer::compute_checksum(unsigned char *packet,int len)
return checksum;
}
+unsigned short int CDynamixelServer::compute_checksum_v2(unsigned char *packet,int len)
+{
+ unsigned short int i,j,crc_accum=0;
+
+ if(packet==NULL)
+ {
+ /* handle exceptions */
+ throw CDynamixelServerException(_HERE_,"Invalid packet structure");
+ }
+ else
+ {
+ if(len<=0)
+ {
+ /* handle exceptions */
+ throw CDynamixelServerException(_HERE_,"Invalid packet length");
+ }
+ else
+ {
+ for(j=0;j<len;j++)
+ {
+ i=((unsigned short)(crc_accum >> 8)^packet[j])&0xFF;
+ crc_accum=(crc_accum<<8)^crc_table[i];
+ }
+ }
+ }
+
+ return crc_accum;
+}
+
+void CDynamixelServer::byte_stuffing(unsigned char *packet_in,int len_in,unsigned char *packet_out,int *len_out)
+{
+ int i,num_bytes=0,stuffed_bytes=0;;
+
+ for(i=0;i<8;i++)
+ packet_out[i]=packet_in[i];
+ for(;i<len_in;i++)
+ {
+ packet_out[i+stuffed_bytes]=packet_in[i];
+ if(num_bytes==0 && packet_in[i]==0xFF)
+ num_bytes++;
+ else if(num_bytes==1 && packet_in[i]==0xFF)
+ num_bytes++;
+ else if(num_bytes==2 && packet_in[i]==0xFD)
+ {
+ stuffed_bytes++;
+ packet_out[i+stuffed_bytes]=0xFD;
+ num_bytes=0;
+ }
+ else
+ num_bytes=0;
+ }
+ *len_out=len_in+stuffed_bytes;
+}
+
+void CDynamixelServer::byte_destuffing(unsigned char *packet_in,int len_in,unsigned char *packet_out,int *len_out)
+{
+ int i,num_bytes=0,stuffed_bytes=0;
+
+ for(i=0;i<9;i++)
+ packet_out[i]=packet_in[i];
+ for(;i<len_in;i++)
+ {
+ packet_out[i-stuffed_bytes]=packet_in[i];
+ if(num_bytes==0 && packet_in[i]==0xFF)
+ num_bytes++;
+ else if(num_bytes==1 && packet_in[i]==0xFF)
+ num_bytes++;
+ else if(num_bytes==2 && packet_in[i]==0xFD)
+ {
+ stuffed_bytes++;
+ i++;
+ num_bytes=0;
+ }
+ else
+ num_bytes=0;
+ }
+ *len_out=len_in-stuffed_bytes;
+}
+
+void CDynamixelServer::handle_error(unsigned char error)
+{
+ std::string error_msg;
+
+ if(error!=0x00)
+ {
+ /* handle exceptions */
+ if(error&0x01)
+ error_msg+="\nThe supply voltage is out of range";
+ if(error&0x02)
+ error_msg+="\nThe goal position is out of range";
+ if(error&0x04)
+ error_msg+="\nThe internal temperature is too high";
+ if(error&0x08)
+ error_msg+="\nThe provided parameter is out of range";
+ if(error&0x10)
+ error_msg+="\nThe packet sent to the dynamixel device had an invalid checksum";
+ if(error&0x20)
+ error_msg+="\nOverload";
+ if(error&0x40)
+ error_msg+="\nInvalid instruction";
+ std::cout << error_msg << std::endl;
+ throw CDynamixelAlarmException(_HERE_,error_msg,0xFE,error);
+ }
+}
+
void *CDynamixelServer::scan_thread(void *param)
{
CDynamixelServer *dyn_server=(CDynamixelServer *)param;
@@ -82,7 +495,6 @@ void *CDynamixelServer::scan_thread(void *param)
int freq=0,id=0;
bool end=false,found=false;
- dyn_server->close();
dyn_server->state=dyn_scanning;
while(!end)
{
@@ -100,22 +512,30 @@ void *CDynamixelServer::scan_thread(void *param)
if(dyn_server->event_server->event_is_set(dyn_server->stop_scan_event_id))
{
dyn_server->event_server->reset_event(dyn_server->stop_scan_event_id);
+ if(dyn_server->scan_version==dyn_version1)
+ dyn_server->devices_v1.clear();
+ else
+ dyn_server->devices_v2.clear();
dyn_server->close();
pthread_exit(NULL);
}
else
{
try{
- dyn_server->ping(id,200);
+ dyn_server->ping(id,20,dyn_server->scan_version);
}catch(CEventTimeoutException &e){
continue;
}
dyn_server->dynamixel_access.enter();
dyn_server->bus_info.baud_rate=frequencies[freq];
device.id=id;
+ device.version=dyn_server->scan_version;
device.used=false;
dyn_server->dynamixel_access.exit();
- dyn_server->devices.push_back(device);
+ if(dyn_server->scan_version==dyn_version1)
+ dyn_server->devices_v1.push_back(device);
+ else
+ dyn_server->devices_v2.push_back(device);
found=true;
}
}
@@ -125,8 +545,18 @@ void *CDynamixelServer::scan_thread(void *param)
break;
}
}
+ end=true;// nothing found
}catch(CException &e){
+ dyn_server->dynamixel_access.enter();
+ if(dyn_server->scan_version==dyn_version1)
+ dyn_server->devices_v1.clear();
+ else
+ dyn_server->devices_v2.clear();
+ dyn_server->state=dyn_created;
+ dyn_server->scan_error=e.what();
+ dyn_server->event_server->set_event(dyn_server->scan_error_event_id);
dyn_server->close();
+ dyn_server->dynamixel_access.exit();
pthread_exit(NULL);
}
}
@@ -275,7 +705,7 @@ void CDynamixelServer::set_bus_id(std::string &bus_id)
}
}
-void CDynamixelServer::start_scan(void)
+void CDynamixelServer::start_scan(dyn_version_t version)
{
if(this->bus_info.bus_id==-1 && this->bus_info.serial.size()==0)
{
@@ -284,7 +714,12 @@ void CDynamixelServer::start_scan(void)
}
else
{
+ this->scan_version=version;
this->stop_scan();
+ if(version==dyn_version1)
+ this->devices_v1.clear();
+ else
+ this->devices_v2.clear();
this->close();
this->thread_server->start_thread(this->scan_thread_id);
}
@@ -322,6 +757,16 @@ std::string CDynamixelServer::get_scan_done_event_id(void)
return this->scan_done_event_id;
}
+std::string CDynamixelServer::get_scan_error_event_id(void)
+{
+ return this->scan_error_event_id;
+}
+
+std::string CDynamixelServer::get_scan_error(void)
+{
+ return this->scan_error;
+}
+
int CDynamixelServer::get_baudrate(void)
{
int baud_rate=0;
@@ -333,31 +778,38 @@ int CDynamixelServer::get_baudrate(void)
return baud_rate;
}
-int CDynamixelServer::get_num_devices(void)
+int CDynamixelServer::get_num_devices(dyn_version_t version)
{
int num=0;
this->dynamixel_access.enter();
- num=this->devices.size();
+ if(version==dyn_version1)
+ num=this->devices_v1.size();
+ else
+ num=this->devices_v2.size();
this->dynamixel_access.exit();
return num;
}
-std::vector<int> CDynamixelServer::get_device_ids(void)
+std::vector<int> CDynamixelServer::get_device_ids(dyn_version_t version)
{
std::vector<int> ids;
unsigned int i=0;
this->dynamixel_access.enter();
- for(i=0;i<this->devices.size();i++)
- ids.push_back(this->devices[i].id);
+ if(version==dyn_version1)
+ for(i=0;i<this->devices_v1.size();i++)
+ ids.push_back(this->devices_v1[i].id);
+ else
+ for(i=0;i<this->devices_v2.size();i++)
+ ids.push_back(this->devices_v2[i].id);
this->dynamixel_access.exit();
return ids;
}
-CDynamixel *CDynamixelServer::get_device(int dev_id)
+CDynamixel *CDynamixelServer::get_device(int dev_id,dyn_version_t version)
{
std::stringstream device_name;
CDynamixel *dynamixel=NULL;
@@ -375,16 +827,32 @@ CDynamixel *CDynamixelServer::get_device(int dev_id)
}
else
{
- for(i=0;i<this->devices.size();i++)
- if(this->devices[i].id==dev_id)
- {
- if(this->devices[i].used)
+ if(version==dyn_version1)
+ {
+ for(i=0;i<this->devices_v1.size();i++)
+ if(this->devices_v1[i].id==dev_id)
{
- /* handle exceptions */
- this->dynamixel_access.exit();
- throw CDynamixelServerException(_HERE_,"Device has already been assigned.");
+ if(this->devices_v1[i].used)
+ {
+ /* handle exceptions */
+ this->dynamixel_access.exit();
+ throw CDynamixelServerException(_HERE_,"Device has already been assigned.");
+ }
}
- }
+ }
+ else
+ {
+ for(i=0;i<this->devices_v2.size();i++)
+ if(this->devices_v2[i].id==dev_id)
+ {
+ if(this->devices_v2[i].used)
+ {
+ /* handle exceptions */
+ this->dynamixel_access.exit();
+ throw CDynamixelServerException(_HERE_,"Device has already been assigned.");
+ }
+ }
+ }
}
this->dynamixel_access.exit();
if(dynamixel==NULL)
@@ -398,34 +866,56 @@ CDynamixel *CDynamixelServer::get_device(int dev_id)
else
{
try{
- this->ping(dev_id,500);
-// }catch(CEventTimeoutException &e){
+ this->ping(dev_id,500,version);
}catch(...){
/* handle exceptions */
throw CDynamixelServerException(_HERE_,"No Dynamixel device found with the specified identifier");
}
this->dynamixel_access.enter();
device_name.str("");
- device_name << "dynamixel_bus_" << this->bus_info.bus_id << "_dev_" << dev_id;
+ device_name << "dynamixel_bus_" << this->bus_info.bus_id << "_dev_" << dev_id << "_v" << version;
name=device_name.str();
dynamixel=new CDynamixel(name);
dynamixel->usb_dev=this->comm_dev;
dynamixel->usb_access=&this->dynamixel_access;
dynamixel->node_address=dev_id;
+ dynamixel->version=version;
dynamixel->usb_rx_event_id=this->comm_dev->get_rx_event_id();
- for(i=0;i<devices.size();i++)
+ if(version==dyn_version1)
{
- if(this->devices[i].id==dev_id)
+ for(i=0;i<devices_v1.size();i++)
{
- this->devices[i].used=true;
- updated=true;
+ if(this->devices_v1[i].id==dev_id)
+ {
+ this->devices_v1[i].used=true;
+ updated=true;
+ }
+ }
+ if(!updated)
+ {
+ device.id=dev_id;
+ device.version=version;
+ device.used=true;
+ this->devices_v1.push_back(device);
}
}
- if(!updated)
+ else
{
- device.id=dev_id;
- device.used=true;
- this->devices.push_back(device);
+ for(i=0;i<devices_v2.size();i++)
+ {
+ if(this->devices_v2[i].id==dev_id)
+ {
+ this->devices_v2[i].used=true;
+ updated=true;
+ }
+ }
+ if(!updated)
+ {
+ device.id=dev_id;
+ device.version=version;
+ device.used=true;
+ this->devices_v2.push_back(device);
+ }
}
this->dynamixel_access.exit();
}
@@ -434,26 +924,40 @@ CDynamixel *CDynamixelServer::get_device(int dev_id)
return dynamixel;
}
-void CDynamixelServer::free_device(int dev_id)
+void CDynamixelServer::free_device(int dev_id,dyn_version_t version)
{
unsigned int i=0;
- for(i=0;i<this->devices.size();i++)
+ if(version==dyn_version1)
{
- if(this->devices[i].id==dev_id)
+ for(i=0;i<this->devices_v1.size();i++)
{
- if(this->devices[i].used)
- this->devices[i].used=false;
+ if(this->devices_v1[i].id==dev_id)
+ {
+ if(this->devices_v1[i].used)
+ this->devices_v1[i].used=false;
+ }
+ }
+ }
+ else
+ {
+ for(i=0;i<this->devices_v2.size();i++)
+ {
+ if(this->devices_v2[i].id==dev_id)
+ {
+ if(this->devices_v2[i].used)
+ this->devices_v2[i].used=false;
+ }
}
}
}
void CDynamixelServer::set_baudrate(int baudrate)
{
+ std::vector< std::vector<unsigned char> > data;
+ std::vector<unsigned char> servo_ids;
TFTDIconfig ftdi_config;
- unsigned char *packet;
unsigned int i=0;
- int length=0;
if(this->comm_dev!=NULL)
{
@@ -465,34 +969,29 @@ void CDynamixelServer::set_baudrate(int baudrate)
else
{
try{
- if(this->devices.size()>0)
+ if(this->devices_v1.size()>0)
{
- length=8+2*devices.size();
- packet=new unsigned char[length];
- packet[0]=0xFF;
- packet[1]=0xFF;
- packet[2]=0XFE;
- packet[3]=length-4;
- packet[4]=0x83;
- packet[5]=0x04;
- packet[6]=0x01;
- for(i=0;i<this->devices.size();i++)
+ servo_ids.clear();
+ data.resize(this->devices_v1.size());
+ for(i=0;i<this->devices_v1.size();i++)
{
- packet[7+i*2]=devices[i].id;
- packet[7+i*2+1]=((2000000/baudrate)-1);
+ servo_ids.push_back(devices_v1[i].id);
+ data[i].clear();
+ data[i].push_back(((2000000/baudrate)-1));
}
- packet[length-1]=0x00;
- packet[length-1]=this->compute_checksum(packet,length);
- // send the data
- this->dynamixel_access.enter();
- if(this->comm_dev->write(packet,length)!=length)
+ this->write_sync(servo_ids,0x04,data,dyn_version1);
+ }
+ if(this->devices_v2.size()>0)
+ {
+ servo_ids.clear();
+ data.resize(this->devices_v2.size());
+ for(i=0;i<this->devices_v2.size();i++)
{
- delete[] packet;
- this->dynamixel_access.exit();
- throw CDynamixelServerException(_HERE_,"Unexpected error while writing to the communication device");
+ servo_ids.push_back(devices_v2[i].id);
+ data[i].clear();
+ data[i].push_back(((2000000/baudrate)-1));
}
- delete[] packet;
- this->dynamixel_access.exit();
+ this->write_sync(servo_ids,0x04,data,dyn_version2);
}
sleep(1);
/* reconfigure the communciations device */
@@ -518,79 +1017,42 @@ void CDynamixelServer::set_baudrate(int baudrate)
}
}
-void CDynamixelServer::action(void)
+void CDynamixelServer::action(dyn_version_t version)
{
- unsigned char packet[6]={0xFF,0xFF,0xFE,0x02,0x05,0x00};
-
- if(this->comm_dev!=NULL)
- {
- packet[5]=0xFA;
-
- this->dynamixel_access.enter();
- if(this->comm_dev->write(packet,6)!=6)
- {
- // handle exceptions
- this->dynamixel_access.exit();
- throw CDynamixelServerException(_HERE_,"Unexpected error while writing to the communication device");
- }
- this->dynamixel_access.exit();
- }
+ if(version==dyn_version1)
+ this->send_instruction_packet_v1(dyn_action,NULL,0);
else
- {
- /* handle exceptions */
- throw CDynamixelServerException(_HERE_,"The communication device is not ready to send information");
- }
+ this->send_instruction_packet_v2(dyn_action,NULL,0);
}
-void CDynamixelServer::ping(int dev_id,int time)
+void CDynamixelServer::ping(int dev_id,int time,dyn_version_t version,unsigned short *model,unsigned char *fw_ver)
{
- unsigned char packet[6]={0xFF,0xFF,0x00,0x02,0x01,0x00};
- std::list<std::string> events;
- unsigned char data[6];
- int num=0,read=0;
+ unsigned char *data,length_v1;
+ unsigned short length_v2;
- if(this->comm_dev!=NULL)
+ if(version==dyn_version1)
{
- this->dynamixel_access.enter();
- packet[2]=dev_id;
- packet[5]=(~(dev_id+3))%256;
- events.push_back(this->comm_dev->get_rx_event_id());
-
- if(this->comm_dev->write(packet,6)!=6)
- {
- /* handle exceptions */
- this->dynamixel_access.exit();
- throw CDynamixelServerException(_HERE_,"Unexpected error while writing to the communication device");
- }
- else
- {
- try{
- do{
- this->event_server->wait_all(events,time);
- num=this->comm_dev->read(&data[read],6-read);
- read+=num;
- }while(read<6);
- }catch(CEventTimeoutException &e){
- this->dynamixel_access.exit();
- throw e;
- }
- }
- this->dynamixel_access.exit();
+ this->send_instruction_packet_v1(dyn_ping,NULL,0,dev_id);
+ this->receive_status_packet_v1(&data,&length_v1);
+ if(data!=NULL)
+ delete[] data;
}
else
{
- /* handle exceptions */
- throw CDynamixelServerException(_HERE_,"The communication device is not ready to send information");
+ this->send_instruction_packet_v2(dyn_ping,NULL,0,dev_id);
+ this->receive_status_packet_v2(&data,&length_v2);
+ if(model!=NULL)
+ *model=data[0]+data[1]*256;
+ if(fw_ver!=NULL)
+ *fw_ver=data[2];
+ if(data!=NULL)
+ delete[] data;
}
}
void CDynamixelServer::close(void)
{
this->dynamixel_access.enter();
- if(this->devices.size()>0)
- {
- this->devices.clear();
- }
if(this->comm_dev!=NULL)
{
this->comm_dev->close();
@@ -602,59 +1064,213 @@ void CDynamixelServer::close(void)
this->dynamixel_access.exit();
}
-void CDynamixelServer::write_sync(std::vector<unsigned char>& servo_ids,unsigned char start_addr, std::vector< std::vector<unsigned char> >& data)
+void CDynamixelServer::write_sync(std::vector<unsigned char>& servo_ids,unsigned short int start_addr, std::vector< std::vector<unsigned char> >& data,dyn_version_t version)
{
- unsigned char *packet;
+ unsigned char *data_int;
unsigned int i,j=0;
- int length=0;
+ int length=0;
- if(this->comm_dev!=NULL)
+ if(servo_ids.size()==0)
{
- try{
- if(servo_ids.size()>0)
- {
- length=8+(data[0].size()+1)*servo_ids.size();
- packet=new unsigned char[length];
- packet[0]=0xFF;
- packet[1]=0xFF;
- packet[2]=0XFE;
- packet[3]=length-4;
- packet[4]=0x83;
- packet[5]=start_addr;
- packet[6]=data[0].size();
-
- for(i=0;i<servo_ids.size();i++)
- {
- packet[7+(data[i].size()+1)*i]= servo_ids[i];
-
- for(j=0;j<data[i].size();j++)
- packet[7+(data[i].size()+1)*i+1+j]=data[i][j];
- }
- packet[length-1]=0x00;
- packet[length-1]=this->compute_checksum(packet,length);
- this->dynamixel_access.enter();
- if(this->comm_dev->write(packet,length)!=length)
- {
- delete[] packet;
- this->dynamixel_access.exit();
- throw CDynamixelServerException(_HERE_,"Unexpected error while writing to the communication device");
- }
- delete[] packet;
- this->dynamixel_access.exit();
- }
- }catch(CException &e){
- /* handle exceptions */
- throw;
+ /* handle exceptions */
+ throw CDynamixelServerException(_HERE_,"No servos specified");
+ }
+ if(servo_ids.size()>data.size())
+ {
+ /* handle exception */
+ throw CDynamixelServerException(_HERE_,"Missing data for some servos");
+ }
+ if(servo_ids.size()<data.size())
+ {
+ /* handle exception */
+ throw CDynamixelServerException(_HERE_,"Too much data blocks or some missing servos");
+ }
+ if(version==dyn_version1)
+ {
+ length=2+(data[0].size()+1)*servo_ids.size();
+ data_int=new unsigned char[length];
+ data_int[0]=(unsigned char)start_addr;
+ data_int[1]=data[0].size();
+ for(i=0;i<servo_ids.size();i++)
+ {
+ data_int[2+(data[i].size()+1)*i]= servo_ids[i];
+ for(j=0;j<data[i].size();j++)
+ data_int[2+(data[i].size()+1)*i+1+j]=data[i][j];
}
- }else
+ this->send_instruction_packet_v1(dyn_sync_write,data_int,length);
+ }
+ else
{
- /* handle exceptions */
- throw CDynamixelServerException(_HERE_,"The communication device is not ready to send information");
+ length=4+(data[0].size()+1)*servo_ids.size();
+ data_int=new unsigned char[length];
+ data_int[0]=start_addr%256;
+ data_int[1]=start_addr/256;
+ data_int[2]=data[0].size()%256;
+ data_int[3]=data[0].size()/256;
+ for(i=0;i<servo_ids.size();i++)
+ {
+ data_int[4+(data[i].size()+1)*i]= servo_ids[i];
+ for(j=0;j<data[i].size();j++)
+ data_int[4+(data[i].size()+1)*i+1+j]=data[i][j];
+ }
+ this->send_instruction_packet_v2(dyn_sync_write,data_int,length);
}
}
+void CDynamixelServer::read_sync(std::vector<unsigned char>& servo_ids,unsigned short int start_addr,unsigned short int length, std::vector< std::vector<unsigned char> >& data,dyn_version_t version)
+{
+ unsigned char *data_int,*data_out,error;
+ unsigned short length_v2;
+ unsigned int i,j=0;
+ int length_int=0;
+ if(servo_ids.size()==0)
+ {
+ /* handle exceptions */
+ throw CDynamixelServerException(_HERE_,"No servos specified");
+ }
+ if(version==dyn_version1)
+ {
+ /* handle exception */
+ throw CDynamixelServerException(_HERE_,"Operation not supported in version 1");
+ }
+ else
+ {
+ length_int=4+servo_ids.size();
+ data_int=new unsigned char[length_int];
+ data_int[0]=start_addr%256;
+ data_int[1]=start_addr/256;
+ data_int[2]=length%256;
+ data_int[3]=length/256;
+ for(i=0;i<servo_ids.size();i++)
+ data_int[4+i]=servo_ids[i];
+ this->send_instruction_packet_v2(dyn_sync_read,data_int,length_int);
+ /* read all the data from all the servos */
+ data.resize(servo_ids.size());
+ for(i=0;i<servo_ids.size();i++)
+ {
+ error=this->receive_status_packet_v2(&data_out,&length_v2);
+ try{
+ this->handle_error(error);
+ if(data_out!=NULL)
+ {
+ data[i].resize(length_v2);
+ for(j=0;j<length_v2;j++)
+ data[i][j]=data_out[j];
+ delete[] data_out;
+ }
+ }catch(CException &e){
+ if(data_out!=NULL)
+ delete[] data_out;
+ }
+ }
+ }
+}
+void CDynamixelServer::write_bulk(std::vector<unsigned char>& servo_ids,std::vector<unsigned short int> &start_addr, std::vector< std::vector<unsigned char> >& data,dyn_version_t version)
+{
+ unsigned char *data_int;
+ unsigned int i;
+ int length=0,offset=0;
+ if(servo_ids.size()==0)
+ {
+ /* handle exceptions */
+ throw CDynamixelServerException(_HERE_,"No servos specified");
+ }
+ if(start_addr.size()!=servo_ids.size())
+ {
+ /* handle exceptions */
+ throw CDynamixelServerException(_HERE_,"Too many or not enough write addresses");
+ }
+ if(data.size()!=servo_ids.size())
+ {
+ /* handle exceptions */
+ throw CDynamixelServerException(_HERE_,"Too many or not enough data blocks");
+ }
+ if(version==dyn_version1)
+ {
+ /* handle exception */
+ throw CDynamixelServerException(_HERE_,"Operation not supported in version 1");
+ }
+ else
+ {
+ length=0;
+ for(i=0;i<servo_ids.size();i++)
+ length+=5+data[i].size();
+ data_int=new unsigned char[length];
+ for(i=0;i<servo_ids.size();i++)
+ {
+ data_int[offset]=servo_ids[i];
+ data_int[offset+1]=start_addr[i]%256;
+ data_int[offset+2]=start_addr[i]/256;
+ data_int[offset+3]=data[i].size()%256;
+ data_int[offset+4]=data[i].size()/256;
+ offset+=5+data[i].size();
+ }
+ this->send_instruction_packet_v2(dyn_sync_read,data_int,length);
+ }
+}
+void CDynamixelServer::read_bulk(std::vector<unsigned char>& servo_ids,std::vector<unsigned short int> &start_addr,std::vector<unsigned short int> &length, std::vector< std::vector<unsigned char> >& data,dyn_version_t version)
+{
+ unsigned char *data_int,*data_out,error;
+ unsigned short int length_v2;
+ unsigned int i,j=0;
+ int length_int=0;
+
+ if(servo_ids.size()==0)
+ {
+ /* handle exceptions */
+ throw CDynamixelServerException(_HERE_,"No servos specified");
+ }
+ if(start_addr.size()!=servo_ids.size())
+ {
+ /* handle exceptions */
+ throw CDynamixelServerException(_HERE_,"Too many or not enough read addresses");
+ }
+ if(length.size()!=servo_ids.size())
+ {
+ /* handle exceptions */
+ throw CDynamixelServerException(_HERE_,"Too many or not enough read lengths");
+ }
+ if(version==dyn_version1)
+ {
+ /* handle exception */
+ throw CDynamixelServerException(_HERE_,"Operation not supported in version 1");
+ }
+ else
+ {
+ length_int=5*servo_ids.size();
+ data_int=new unsigned char[length_int];
+ for(i=0;i<servo_ids.size();i++)
+ {
+ data_int[i*5]=servo_ids[i];
+ data_int[i*5+1]=start_addr[i]%256;
+ data_int[i*5+2]=start_addr[i]/256;
+ data_int[i*5+3]=length[i]%256;
+ data_int[i*5+4]=length[i]/256;
+ }
+ this->send_instruction_packet_v2(dyn_sync_read,data_int,length_int);
+ /* read all the data from all the servos */
+ data.resize(servo_ids.size());
+ for(i=0;i<servo_ids.size();i++)
+ {
+ error=this->receive_status_packet_v2(&data_out,&length_v2);
+ try{
+ this->handle_error(error);
+ if(data_out!=NULL)
+ {
+ data[i].resize(length_v2);
+ for(j=0;j<length_v2;j++)
+ data[i][j]=data_out[j];
+ delete[] data_out;
+ }
+ }catch(CException &e){
+ if(data_out!=NULL)
+ delete[] data_out;
+ }
+ }
+ }
+}
diff --git a/src/dynamixelserver.h b/src/dynamixelserver.h
index 18d866a6dab2bcfc9e4581150570cab4556c79af..f941278d33ece74ec8aff68edd7e797ddbdabd2d 100644
--- a/src/dynamixelserver.h
+++ b/src/dynamixelserver.h
@@ -52,7 +52,8 @@ typedef struct
typedef struct
{
- unsigned char id;
+ unsigned char id;
+ dyn_version_t version;
bool used;
}TDynDevice;
@@ -89,6 +90,11 @@ class CDynamixelServer
*
*/
std::string scan_done_event_id;
+ /**
+ * \brief
+ *
+ */
+ std::string scan_error_event_id;
/**
* \brief
*
@@ -123,7 +129,42 @@ class CDynamixelServer
* \brief
*
*/
- std::vector<TDynDevice> devices;
+ std::vector<TDynDevice> devices_v1;
+ /**
+ * \brief
+ *
+ */
+ std::vector<TDynDevice> devices_v2;
+ /**
+ * \brief
+ *
+ */
+ std::string scan_error;
+ /**
+ * \brief
+ *
+ */
+ dyn_version_t scan_version;
+ /**
+ * \brief
+ *
+ */
+ void send_instruction_packet_v1(dyn_inst_t inst,unsigned char *data=NULL,unsigned char len=0,unsigned char id=0xFE);
+ /**
+ * \brief
+ *
+ */
+ void send_instruction_packet_v2(dyn_inst_t inst,unsigned char *data=NULL,unsigned short int len=0,unsigned char id=0xFE);
+ /**
+ * \brief
+ *
+ */
+ unsigned char receive_status_packet_v1(unsigned char **data,unsigned char *len);
+ /**
+ * \brief
+ *
+ */
+ unsigned char receive_status_packet_v2(unsigned char **data,unsigned short int *len);
protected:
/**
* \brief
@@ -149,7 +190,12 @@ class CDynamixelServer
* \brief
*
*/
- unsigned char compute_checksum(unsigned char *packet,int len);
+ static void byte_stuffing(unsigned char *packet_in,int len_in,unsigned char *packet_out,int *len_out);
+ /**
+ * \brief
+ *
+ */
+ static void byte_destuffing(unsigned char *packet_in,int len_in,unsigned char *packet_out,int *len_out);
/**
* \brief
*
@@ -160,12 +206,27 @@ class CDynamixelServer
*
*/
void set_bus_id(std::string &bus_id);
+ /**
+ * \brief
+ *
+ */
+ void handle_error(unsigned char error);
public:
/**
* \brief
*
*/
static CDynamixelServer *instance(void);
+ /**
+ * \brief
+ *
+ */
+ static unsigned char compute_checksum_v1(unsigned char *packet,int len);
+ /**
+ * \brief
+ *
+ */
+ static unsigned short int compute_checksum_v2(unsigned char *packet,int len);
/**
* \brief
*
@@ -200,7 +261,7 @@ class CDynamixelServer
* \brief
*
*/
- void start_scan(void);
+ void start_scan(dyn_version_t version=dyn_version1);
/**
* \brief
*
@@ -216,6 +277,16 @@ class CDynamixelServer
*
*/
std::string get_scan_done_event_id(void);
+ /**
+ * \brief
+ *
+ */
+ std::string get_scan_error_event_id(void);
+ /**
+ * \brief
+ *
+ */
+ std::string get_scan_error(void);
/**
* \brief
*
@@ -225,32 +296,32 @@ class CDynamixelServer
* \brief
*
*/
- int get_num_devices(void);
+ int get_num_devices(dyn_version_t version=dyn_version1);
/**
* \brief
*
*/
- std::vector<int> get_device_ids(void);
+ std::vector<int> get_device_ids(dyn_version_t version=dyn_version1);
/**
* \brief
*
*/
- CDynamixel *get_device(int dev_id);
+ CDynamixel *get_device(int dev_id,dyn_version_t version=dyn_version1);
/**
* \brief
*
*/
- void free_device(int dev_id);
+ void free_device(int dev_id,dyn_version_t version=dyn_version1);
/**
* \brief
*
*/
- void action(void);
+ void action(dyn_version_t version=dyn_version1);
/**
* \brief
*
*/
- void ping(int dev_id,int time=100);
+ void ping(int dev_id,int time=100,dyn_version_t version=dyn_version1,unsigned short *model=NULL,unsigned char *fw_ver=NULL);
/**
* \brief
*
@@ -260,7 +331,22 @@ class CDynamixelServer
* \brief
*
*/
- void write_sync(std::vector<unsigned char>& servo_ids,unsigned char start_addr, std::vector< std::vector<unsigned char> >& data);
+ void write_sync(std::vector<unsigned char>& servo_ids,unsigned short int start_addr, std::vector< std::vector<unsigned char> >& data,dyn_version_t version=dyn_version1);
+ /**
+ * \brief
+ *
+ */
+ void read_sync(std::vector<unsigned char>& servo_ids,unsigned short int start_addr,unsigned short int length, std::vector< std::vector<unsigned char> >& data,dyn_version_t version=dyn_version2);
+ /**
+ * \brief
+ *
+ */
+ void write_bulk(std::vector<unsigned char>& servo_ids,std::vector<unsigned short int> &start_addr, std::vector< std::vector<unsigned char> >& data,dyn_version_t version=dyn_version2);
+ /**
+ * \brief
+ *
+ */
+ void read_bulk(std::vector<unsigned char>& servo_ids,std::vector<unsigned short int> &start_addr,std::vector<unsigned short int> &length, std::vector< std::vector<unsigned char> >& data,dyn_version_t version=dyn_version2);
};
#endif
diff --git a/src/examples/test_dynamixel_server.cpp b/src/examples/test_dynamixel_server.cpp
index 86e69fc9ef4d1a717f7055190a302fe5f4e4bdf9..12e416f7a70903d22e5c1c3cc84a66a7399342d2 100644
--- a/src/examples/test_dynamixel_server.cpp
+++ b/src/examples/test_dynamixel_server.cpp
@@ -6,10 +6,12 @@ int main(int argc, char *argv[])
{
CDynamixelServer *dyn_server=CDynamixelServer::instance();
CEventServer *event_server=CEventServer::instance();
- int num_buses=0,num_dev=0,baudrate=0,i=0;
+ int num_buses=0,num_dev=0,baudrate=0,i=0,event_id;
std::list<std::string> events;
std::vector<float> position;
std::vector<int> devices;
+ unsigned short int model;
+ unsigned char margin;
CDynamixel *dyn_motor;
num_buses=dyn_server->get_num_buses();
@@ -17,17 +19,33 @@ int main(int argc, char *argv[])
if(num_buses>0)
{
events.push_back(dyn_server->get_scan_done_event_id());
+ events.push_back(dyn_server->get_scan_error_event_id());
dyn_server->config_bus(0,1000000);
dyn_server->start_scan();
- event_server->wait_first(events);
- num_dev=dyn_server->get_num_devices();
- std::cout << "Num. devices: " << num_dev << std::endl;
- baudrate=dyn_server->get_baudrate();
- std::cout << "Baudrate: " << baudrate << " bps" << std::endl;
- devices=dyn_server->get_device_ids();
- for(i=0;i<num_dev;i++)
- std::cout << "Device " << i << ": id -> " << devices[i] << std::endl;
- dyn_motor=dyn_server->get_device(devices[0]);
+ event_id=event_server->wait_first(events);
+ if(event_id==0)
+ {
+ num_dev=dyn_server->get_num_devices();
+ std::cout << "Num. devices: " << num_dev << std::endl;
+ baudrate=dyn_server->get_baudrate();
+ std::cout << "Baudrate: " << baudrate << " bps" << std::endl;
+ devices=dyn_server->get_device_ids();
+ for(i=0;i<num_dev;i++)
+ {
+ dyn_motor=dyn_server->get_device(devices[i]);
+ dyn_motor->write_byte_register(0x1A,0x10);
+ dyn_motor->read_word_register(0x00,&model);
+ dyn_motor->read_byte_register(0x1A,&margin);
+ dyn_motor->write_word_register(0x1E,0x03FF);
+ sleep(2);
+ dyn_motor->write_word_register(0x1E,0x000);
+ sleep(2);
+ std::cout << "Device " << std::dec << i << ": id -> " << devices[i] << " model: " << std::hex << model << " margin: " << (int)margin << std::endl;
+ dyn_server->free_device(devices[i]);
+ }
+ }
+ else
+ std::cout << "Error while scanning the bus: " << dyn_server->get_scan_error() << std::endl;
try{
dyn_server->config_bus(0,1000000);
dyn_server->start_scan();
@@ -36,12 +54,32 @@ int main(int argc, char *argv[])
dyn_server->stop_scan();
std::cout << "Scanning canceled !!!" << std::endl;
}
- num_dev=dyn_server->get_num_devices();
- std::cout << "Num. devices: " << num_dev << std::endl;
- baudrate=dyn_server->get_baudrate();
- std::cout << "Baudrate: " << baudrate << " bps" << std::endl;
- devices=dyn_server->get_device_ids();
- for(i=0;i<num_dev;i++)
- std::cout << "Device " << i << ": id -> " << devices[i] << std::endl;
+ /* scan for version 2 devices */
+ dyn_server->config_bus(0,1000000);
+ dyn_server->start_scan(dyn_version2);
+ event_id=event_server->wait_first(events);
+ if(event_id==0)
+ {
+ num_dev=dyn_server->get_num_devices(dyn_version2);
+ std::cout << "Num. devices: " << num_dev << std::endl;
+ baudrate=dyn_server->get_baudrate();
+ std::cout << "Baudrate: " << baudrate << " bps" << std::endl;
+ devices=dyn_server->get_device_ids(dyn_version2);
+ for(i=0;i<num_dev;i++)
+ {
+ dyn_motor=dyn_server->get_device(devices[i],dyn_version2);
+ dyn_motor->write_byte_register(0x1A,0x10);
+ dyn_motor->read_word_register(0x00,&model);
+ dyn_motor->read_byte_register(0x1A,&margin);
+ dyn_motor->write_word_register(0x1E,0x03FF);
+ sleep(2);
+ dyn_motor->write_word_register(0x1E,0x000);
+ sleep(2);
+ std::cout << "Device " << std::dec << i << ": id -> " << devices[i] << " model: " << std::hex << model << " margin: " << (int)margin << std::endl;
+ dyn_server->free_device(devices[i],dyn_version2);
+ }
+ }
+ else
+ std::cout << "Error while scanning the bus: " << dyn_server->get_scan_error() << std::endl;
}
}