Skip to content
Snippets Groups Projects
Commit f2409c36 authored by Sergi Hernandez's avatar Sergi Hernandez
Browse files

Added a check of the desired address in all the read and write functions.

parent cfc0ada6
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
Showing
with 217 additions and 190 deletions
src/dynamixel.cpp
+ 217
− 190
View file @ f2409c36
......@@ -124,7 +124,7 @@ unsigned char CDynamixel::receive_status_packet_v1(unsigned char **data,unsigned
do{
if((num=this->usb_dev->get_num_data())==0)
{
this->event_server->wait_all(events,20);
this->event_server->wait_all(events,50);
num=this->usb_dev->get_num_data();
}
if((read+num)>1024)
......@@ -145,7 +145,7 @@ unsigned char CDynamixel::receive_status_packet_v1(unsigned char **data,unsigned
{
if((num=this->usb_dev->get_num_data())==0)
{
this->event_server->wait_all(events,20);
this->event_server->wait_all(events,50);
num=this->usb_dev->get_num_data();
}
if((read-start+num)>length)
......@@ -209,7 +209,7 @@ unsigned char CDynamixel::receive_status_packet_v2(unsigned char **data,unsigned
do{
if((num=this->usb_dev->get_num_data())==0)
{
this->event_server->wait_all(events,20);
this->event_server->wait_all(events,50);
num=this->usb_dev->get_num_data();
}
if((read+num)>256)
......@@ -230,7 +230,7 @@ unsigned char CDynamixel::receive_status_packet_v2(unsigned char **data,unsigned
{
if((num=this->usb_dev->get_num_data())==0)
{
this->event_server->wait_all(events,20);
this->event_server->wait_all(events,50);
num=this->usb_dev->get_num_data();
}
if((read-start+num)>256)
......@@ -465,31 +465,34 @@ void CDynamixel::read_byte_register(unsigned short int address,unsigned char *va
unsigned char *data,error,length_v1,cmd[4];
unsigned short length_v2;
if(version==dyn_version1)
{
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
if(address!=(unsigned short int)-1)
{
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;
if(version==dyn_version1)
{
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
{
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;
}
}
}
......@@ -498,31 +501,34 @@ void CDynamixel::read_word_register(unsigned short int address,unsigned short in
unsigned char *data,error,length_v1,cmd[4];
unsigned short length_v2;
if(version==dyn_version1)
if(address!=(unsigned short int)-1)
{
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
{
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;
if(version==dyn_version1)
{
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
{
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;
}
}
}
......@@ -531,24 +537,27 @@ void CDynamixel::write_byte_register(unsigned short int address, unsigned char v
unsigned char *data,error,length_v1,cmd[3];
unsigned short length_v2;
if(version==dyn_version1)
{
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
if(address!=(unsigned short int)-1)
{
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(version==dyn_version1)
{
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
{
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);
}
if(data!=NULL)
delete[] data;
this->handle_error(error);
}
void CDynamixel::write_word_register(unsigned short int address, unsigned short int value)
......@@ -556,26 +565,29 @@ void CDynamixel::write_word_register(unsigned short int address, unsigned short
unsigned char *data,error,length_v1,cmd[4];
unsigned short length_v2;
if(version==dyn_version1)
if(address!=(unsigned short int)-1)
{
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
{
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(version==dyn_version1)
{
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
{
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);
}
if(data!=NULL)
delete[] data;
this->handle_error(error);
}
void CDynamixel::registered_byte_write(unsigned short int address, unsigned char value)
......@@ -583,51 +595,57 @@ void CDynamixel::registered_byte_write(unsigned short int address, unsigned char
unsigned char *data,error,length_v1,cmd[3];
unsigned short length_v2;
if(version==dyn_version1)
if(address!=(unsigned short int)-1)
{
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
{
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(version==dyn_version1)
{
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
{
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);
}
if(data!=NULL)
delete[] data;
this->handle_error(error);
}
}
void CDynamixel::registered_word_write(unsigned short int address, unsigned short int value)
{
unsigned char *data,error,length_v1,cmd[4];
unsigned short length_v2;
if(version==dyn_version1)
if(address==(unsigned short int)-1)
{
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
{
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(version==dyn_version1)
{
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
{
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);
}
if(data!=NULL)
delete[] data;
this->handle_error(error);
}
void CDynamixel::registered_write(unsigned short int address, unsigned char *values,unsigned int length)
......@@ -635,30 +653,33 @@ void CDynamixel::registered_write(unsigned short int address, unsigned char *val
unsigned char *data,error,length_v1,*cmd;
unsigned short length_v2,i;
if(version==dyn_version1)
{
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
if(address!=(unsigned short int)-1)
{
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(version==dyn_version1)
{
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
{
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);
}
if(data!=NULL)
delete[] data;
this->handle_error(error);
}
void CDynamixel::write_registers(unsigned short int address, unsigned char *values, unsigned int length)
......@@ -666,30 +687,33 @@ void CDynamixel::write_registers(unsigned short int address, unsigned char *valu
unsigned char *data,error,length_v1,*cmd;
unsigned short length_v2,i;
if(version==dyn_version1)
{
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
if(address!=(unsigned short int)-1)
{
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(version==dyn_version1)
{
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
{
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);
}
if(data!=NULL)
delete[] data;
this->handle_error(error);
}
void CDynamixel::read_registers(unsigned short int address, unsigned char *values, unsigned int length)
......@@ -697,32 +721,35 @@ void CDynamixel::read_registers(unsigned short int address, unsigned char *value
unsigned char *data,error,length_v1,cmd[4];
unsigned short length_v2,i;
if(version==dyn_version1)
{
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
if(address!=(unsigned short int)-1)
{
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;
if(version==dyn_version1)
{
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;
}
}
}
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment