Commit bc2bcfde authored by Irene Garcia Camacho's avatar Irene Garcia Camacho

Moved the packet process loop to the reception thread.

parent cd5cdf04
......@@ -142,7 +142,11 @@ void *CDynamixelSlave::process_packets_thread(void *params)
new_data=new unsigned char[num_bytes];
memcpy(new_data,packet,num_bytes);
slave->packets.push_back(new_data);
slave->event_server->set_event(slave->new_packet_available_event_id);
if(slave->dyn_ver==dyn_version1)
slave->dynamixel_loop_v1();
else
slave->dynamixel_loop_v2();
//slave->event_server->set_event(slave->new_packet_available_event_id);
num_bytes=0;
}
}
......@@ -185,7 +189,7 @@ void *CDynamixelSlave::dynamixel_loop_thread(void *params)
void CDynamixelSlave::handle_error(unsigned char error)
{
std::cout << "error!!!!!!!!!!" << std::endl;
}
void CDynamixelSlave::start(void)
......
......@@ -13,27 +13,49 @@ unsigned char rx28_servo_data[SERVO_MEMORY_SIZE]={0x1c,0x00,0x00,0x01,0x22,0xFA,
0x00,0x00,0x78,0x19,0x00,0x00,0x00,0x00,
0x20,0x00};
std::string serial="A400gavm";
std::string serial="AI027ZFC";
void on_ping(void)
void on_ping_03(void)
{
std::cout << "device pinged" << std::endl;
std::cout << "servo 3 pinged" << std::endl;
}
unsigned char on_read(unsigned short int address, unsigned short int length, unsigned char *data)
unsigned char on_read_03(unsigned short int address, unsigned short int length, unsigned char *data)
{
unsigned char num;
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;
}
std::cout << "read operation at address " << address << " with length " << length << std::endl;
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++)
rx28_servo_data[i]=data[num];
return 0x00;
}
void on_ping_01(void)
{
std::cout << "servo 1 pinged" << std::endl;
}
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;
}
unsigned char on_write(unsigned short int address, unsigned short int length, unsigned char *data)
unsigned char on_write_01(unsigned short int address, unsigned short int length, unsigned char *data)
{
std::cout << "wrirte operation at address " << address << " with length " << length << std::endl;
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++)
rx28_servo_data[i]=data[num];
......@@ -51,8 +73,9 @@ int main(int argc, char *argv[])
std::cout << "Num. buses: " << num_buses << std::endl;
if(num_buses>0)
{
slave.config_bus(serial,1000000);
slave.add_slave(0x10,boost::bind(&on_ping),boost::bind(&on_read,_1,_2,_3),boost::bind(&on_write,_1,_2,_3));
slave.config_bus(serial,500000);
slave.add_slave(0x01,boost::bind(&on_ping_01),boost::bind(&on_read_01,_1,_2,_3),boost::bind(&on_write_01,_1,_2,_3));
slave.add_slave(0x03,boost::bind(&on_ping_03),boost::bind(&on_read_03,_1,_2,_3),boost::bind(&on_write_03,_1,_2,_3));
sleep(10000);
std::cout << "exit" << std::endl;
}
......
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