Commit 9c2a8f7d authored by Sergi Hernandez's avatar Sergi Hernandez

Solved some problems when the communication is not reliable.

parent 0bf31506
......@@ -31,11 +31,11 @@ void *CDynamixelSlave::process_packets_thread(void *params)
CDynamixelSlave *slave=(CDynamixelSlave *)params;
std::list<std::string> events;
int event_index,num,i;
unsigned char data[256],new_data[256];
unsigned char data[64*1024],new_data[64*1024];
bool end=false;
static bool data_phase=false;
static unsigned char packet[256];
static unsigned char packet[64*1024];
static int num_bytes=0,length=0;
// wait until the comm device becomes valid
......@@ -59,6 +59,7 @@ void *CDynamixelSlave::process_packets_thread(void *params)
// process the incomming data
slave->comm_access.enter();
num=slave->comm_dev->get_num_data();
std::cout << "Num data: " << num << std::endl;
if(slave->comm_dev->read(data,num)!=num)
{
slave->comm_access.exit();
......@@ -69,6 +70,7 @@ void *CDynamixelSlave::process_packets_thread(void *params)
slave->comm_access.exit();
for(i=0;i<num;i++)
{
std::cout << "num bytes: " << num_bytes << std::endl;
if(!data_phase)
{
switch(num_bytes)
......@@ -132,6 +134,7 @@ void *CDynamixelSlave::process_packets_thread(void *params)
}
else// data phase
{
std::cout << "data phase: " << length << std::endl;
packet[num_bytes]=data[i];
num_bytes++;
length--;
......@@ -210,7 +213,7 @@ void CDynamixelSlave::dynamixel_loop_v1(void)
{
unsigned short int length,address;
unsigned char error,id,prev_id;;
unsigned char *data,*write_data,read_data[256];
unsigned char *data,*write_data,read_data[64*1024];
unsigned int i=0;
data=this->packets[0];
......@@ -327,7 +330,7 @@ void CDynamixelSlave::dynamixel_loop_v2(void)
{
unsigned char error,prev_id,id;
unsigned short int length,address,checksum,checksum_pkt;
unsigned char *data,*write_data,read_data[256];
unsigned char *data,*write_data,read_data[64*1024];
unsigned int i=0;
data=this->packets[0];
......
......@@ -10,7 +10,7 @@ unsigned char rx28_servo_data[SERVO_MEMORY_SIZE]={0x1c,0x00,0x00,0x01,0x22,0xFA,
0x02,0x24,0x24,0x00,0x00,0x00,0x00,0x00,
0x00,0x00,0x01,0x01,0x20,0x20,0x00,0x00,
0x00,0x00,0xFF,0x03,0x00,0x00,0x00,0x00,
0x00,0x00,0x78,0x19,0x00,0x00,0x00,0x00,
0x00,0x00,0x78,0x29,0x00,0x00,0x00,0x00,
0x20,0x00};
std::string serial="AI027ZFC";
......@@ -23,17 +23,22 @@ void on_ping_03(void)
unsigned char on_read_03(unsigned short int address, unsigned short int length, unsigned char *data)
{
std::cout << "servo 3: read operation at address " << address << " with length " << length << std::endl;
for(unsigned int i=address,num=0;i<address+length || i<SERVO_MEMORY_SIZE;i++,num++)
data[num]=rx28_servo_data[i];
return 0x00;
if(address<SERVO_MEMORY_SIZE)
{
std::cout << "do read" << std::endl;
for(unsigned int i=address,num=0;i<address+length && i<SERVO_MEMORY_SIZE;i++,num++)
data[num]=rx28_servo_data[i];
return 0x00;
}
else
return 0x01;
}
unsigned char on_write_03(unsigned short int address, unsigned short int length, unsigned char *data)
{
std::cout << "servo 3: wrirte operation at address " << address << " with length " << length << std::endl;
for(unsigned int i=address,num=0;i<address+length || i<SERVO_MEMORY_SIZE;i++,num++)
for(unsigned int i=address,num=0;i<address+length && i<SERVO_MEMORY_SIZE;i++,num++)
rx28_servo_data[i]=data[num];
return 0x00;
......@@ -47,17 +52,22 @@ void on_ping_01(void)
unsigned char on_read_01(unsigned short int address, unsigned short int length, unsigned char *data)
{
std::cout << "servo 1: read operation at address " << address << " with length " << length << std::endl;
for(unsigned int i=address,num=0;i<address+length || i<SERVO_MEMORY_SIZE;i++,num++)
data[num]=rx28_servo_data[i];
return 0x00;
if(address<SERVO_MEMORY_SIZE)
{
std::cout << "do read" << std::endl;
for(unsigned int i=address,num=0;i<address+length && i<SERVO_MEMORY_SIZE;i++,num++)
data[num]=rx28_servo_data[i];
return 0x00;
}
else
return 0x01;
}
unsigned char on_write_01(unsigned short int address, unsigned short int length, unsigned char *data)
{
std::cout << "servo 1: wrirte operation at address " << address << " with length " << length << std::endl;
for(unsigned int i=address,num=0;i<address+length || i<SERVO_MEMORY_SIZE;i++,num++)
for(unsigned int i=address,num=0;i<address+length && i<SERVO_MEMORY_SIZE;i++,num++)
rx28_servo_data[i]=data[num];
return 0x00;
......
Markdown is supported
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