Commit 11997e3d authored by Sergi Hernandez's avatar Sergi Hernandez

Moved some static variables of the thread to the class to allow multiple instances of the class.

parent ab197094
......@@ -21,6 +21,9 @@ CDynamixelSlave::CDynamixelSlave(const std::string& cont_id,dyn_version_t dyn_ve
this->dyn_ver=dyn_ver;
this->comm_dev=NULL;
this->data_phase=false;
this->num_bytes=0;
this->length=0;
}
void *CDynamixelSlave::process_packets_thread(void *params)
......@@ -31,10 +34,6 @@ void *CDynamixelSlave::process_packets_thread(void *params)
unsigned char data[1024];
bool end=false;
static bool data_phase=false;
static unsigned char packet[64*1024];
static int num_bytes=0,length=0;
// wait until the comm device becomes valid
while(!end)
{
......@@ -71,81 +70,81 @@ void *CDynamixelSlave::process_packets_thread(void *params)
slave->comm_access.exit();
for(i=0;i<num;i++)
{
if(!data_phase)
if(!slave->data_phase)
{
switch(num_bytes)
switch(slave->num_bytes)
{
case 0: if(data[i]==0xFF)
{
packet[num_bytes]=data[i];
num_bytes++;
slave->packet[slave->num_bytes]=data[i];
slave->num_bytes++;
}
break;
case 1: if(data[i]==0xFF)
{
packet[num_bytes]=data[i];
num_bytes++;
slave->packet[slave->num_bytes]=data[i];
slave->num_bytes++;
}
else
num_bytes--;
slave->num_bytes--;
break;
case 2: if(data[i]==0xFD)// version 2 header
{
if(slave->dyn_ver==dyn_version2)// the module is configured for version 2
{
packet[num_bytes]=data[i];
num_bytes++;
slave->packet[slave->num_bytes]=data[i];
slave->num_bytes++;
}
else
num_bytes=0;// ignore packet and restart synchronization
slave->num_bytes=0;// ignore packet and restart synchronization
}
else if(data[i]!=0xFF)
{
packet[num_bytes]=data[i];
num_bytes++;
slave->packet[slave->num_bytes]=data[i];
slave->num_bytes++;
}
break;
case 3: packet[num_bytes]=data[i];
case 3: slave->packet[slave->num_bytes]=data[i];
if(slave->dyn_ver==dyn_version1)
{
length=data[i];
num_bytes++;
slave->length=data[i];
slave->num_bytes++;
/* read packet_data */
data_phase=true;
slave->data_phase=true;
}
else
num_bytes++;
slave->num_bytes++;
break;
case 4: packet[num_bytes]=data[i];
num_bytes++;
case 4: slave->packet[slave->num_bytes]=data[i];
slave->num_bytes++;
break;
case 5: packet[num_bytes]=data[i];
num_bytes++;
length=data[i];
case 5: slave->packet[slave->num_bytes]=data[i];
slave->num_bytes++;
slave->length=data[i];
break;
case 6: packet[num_bytes]=data[i];
num_bytes++;
length+=(data[i]<<8);
case 6: slave->packet[slave->num_bytes]=data[i];
slave->num_bytes++;
slave->length+=(data[i]<<8);
/* read packet_data */
data_phase=true;
slave->data_phase=true;
break;
default: break;
}
}
else// data phase
{
packet[num_bytes]=data[i];
num_bytes++;
length--;
if(length==0)
slave->packet[slave->num_bytes]=data[i];
slave->num_bytes++;
slave->length--;
if(slave->length==0)
{
data_phase=false;
memcpy(slave->new_packet,packet,num_bytes);
slave->data_phase=false;
memcpy(slave->new_packet,slave->packet,slave->num_bytes);
if(slave->dyn_ver==dyn_version1)
slave->dynamixel_loop_v1();
else
slave->dynamixel_loop_v2();
num_bytes=0;
slave->num_bytes=0;
}
}
}
......
......@@ -101,7 +101,31 @@ class CDynamixelSlave
*
*/
unsigned char slave_answer[1024];
/**
* \brief
*
*/
unsigned int slave_answer_length;
/**
* \brief
*
*/
bool data_phase;
/**
* \brief
*
*/
unsigned char packet[64*1024];
/**
* \brief
*
*/
unsigned int num_bytes;
/**
* \brief
*
*/
unsigned int length;
protected:
/**
* \brief mutual exclusion mechanism to access the usb
......
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