diff --git a/dynamixel_base/include/dynamixel_master.h b/dynamixel_base/include/dynamixel_master.h
index 53345e783e670312770579249a82b04ce1571939..6c8c28e81b7843c9f5e6da55ec95038fdbf56269 100644
--- a/dynamixel_base/include/dynamixel_master.h
+++ b/dynamixel_base/include/dynamixel_master.h
@@ -62,7 +62,7 @@ typedef struct
* \brief
*
*/
- unsigned char received_bytes;
+ unsigned short int received_bytes;
/**
* \brief
*
@@ -92,12 +92,7 @@ typedef struct
* \brief
*
*/
- unsigned char sync_num_dev_done;
- /**
- * \brief
- *
- */
- unsigned char bulk_num_dev_done;
+ unsigned char packet_bytes;
}TDynamixelMaster;
/* public functions */
diff --git a/dynamixel_base/src/dynamixel_master.c b/dynamixel_base/src/dynamixel_master.c
index bd61ade6dc3df8bd4ead424cbb43e0e3071522f5..2b3e163eb8f75132b0a2d396a05831ddcc72ea34 100644
--- a/dynamixel_base/src/dynamixel_master.c
+++ b/dynamixel_base/src/dynamixel_master.c
@@ -7,21 +7,26 @@ unsigned char dyn_master_irq_receive_cb(TDynamixelMaster *dyn_master,unsigned ch
{
if(dyn_master->comm_dev->time!=0x00000000)
time_set_timeout(dyn_master->comm_dev->time,dyn_master->rx_timeout_ms*1000);
- switch(dyn_master->received_bytes)
+ switch(dyn_master->packet_bytes)
{
case 0: if(byte==0xFF)
{
dyn_master->rx_buffer[dyn_master->received_bytes]=byte;
dyn_master->received_bytes++;
+ dyn_master->packet_bytes++;
}
break;
case 1: if(byte==0xFF)
{
dyn_master->rx_buffer[dyn_master->received_bytes]=byte;
dyn_master->received_bytes++;
+ dyn_master->packet_bytes++;
}
else
+ {
dyn_master->received_bytes--;
+ dyn_master->packet_bytes--;
+ }
break;
case 2: if(byte==0xFD)// version 2 header
{
@@ -29,44 +34,55 @@ unsigned char dyn_master_irq_receive_cb(TDynamixelMaster *dyn_master,unsigned ch
{
dyn_master->rx_buffer[dyn_master->received_bytes]=byte;
dyn_master->received_bytes++;
+ dyn_master->packet_bytes++;
}
else
- dyn_master->received_bytes=0;// ignore packet and restart synchronization
+ {
+ dyn_master->received_bytes-=2;// ignore packet and restart synchronization
+ dyn_master->packet_bytes=0;
+ }
}
else if(byte!=0xFF)
{
dyn_master->rx_buffer[dyn_master->received_bytes]=byte;
dyn_master->received_bytes++;
+ dyn_master->packet_bytes++;
}
break;
case 3: dyn_master->rx_buffer[dyn_master->received_bytes]=byte;
if(dyn_master->version==DYN_VER1)
{
dyn_master->op_length=byte;
- dyn_master->received_bytes=0;
/* finish reception by IRQ */
comm_cancel_irq_receive(dyn_master->comm_dev);
/* enable dma RX */
- comm_receive_dma(dyn_master->comm_dev,&dyn_master->rx_buffer[4],dyn_master->op_length);
+ comm_receive_dma(dyn_master->comm_dev,&dyn_master->rx_buffer[dyn_master->received_bytes+1],dyn_master->op_length);
+ dyn_master->received_bytes+=dyn_master->op_length+1;
+ dyn_master->packet_bytes=0;
}
else
+ {
dyn_master->received_bytes++;
+ dyn_master->packet_bytes++;
+ }
break;
case 4: dyn_master->rx_buffer[dyn_master->received_bytes]=byte;
dyn_master->received_bytes++;
+ dyn_master->packet_bytes++;
break;
case 5: dyn_master->rx_buffer[dyn_master->received_bytes]=byte;
dyn_master->received_bytes++;
dyn_master->op_length=byte;
+ dyn_master->packet_bytes++;
break;
case 6: dyn_master->rx_buffer[dyn_master->received_bytes]=byte;
- dyn_master->received_bytes++;
dyn_master->op_length+=(byte<<8);
- dyn_master->received_bytes=0;
/* finish reception by IRQ */
comm_cancel_irq_receive(dyn_master->comm_dev);
/* enable dma RX */
- comm_receive_dma(dyn_master->comm_dev,&dyn_master->rx_buffer[7],dyn_master->op_length);
+ comm_receive_dma(dyn_master->comm_dev,&dyn_master->rx_buffer[dyn_master->received_bytes+1],dyn_master->op_length);
+ dyn_master->received_bytes+=dyn_master->op_length+1;
+ dyn_master->packet_bytes=0;
break;
default: break;
}
@@ -102,12 +118,13 @@ unsigned char dyn_master_dma_receive_cb(TDynamixelMaster *dyn_master)
if(dyn_master->comm_dev->time!=0x00000000)
time_cancel_timeout(dyn_master->comm_dev->time);
dyn_master->packet_ready=0x01;
+ dyn_master->received_bytes=0;
}
else
{
/* enable RX interrupts */
comm_receive_irq(dyn_master->comm_dev,0);
- dyn_master->packet_ready=0x01;
+// dyn_master->packet_ready=0x01;
}
}
@@ -219,6 +236,7 @@ unsigned char dyn_master_wait_reception(TDynamixelMaster *master)
}
}
master->packet_ready=0x00;
+ return DYN_SUCCESS;
// check the input packet checksum
if(master->version==DYN_VER1)
{
@@ -502,7 +520,7 @@ unsigned char dyn_master_start_sync_read(TDynamixelMaster *master,unsigned char
unsigned char dyn_master_is_sync_read_done(TDynamixelMaster *master,unsigned char num,unsigned char *ids,unsigned short int address, unsigned short int length, unsigned char * const data[])
{
- unsigned char error=DYN_COMM_ERROR;
+ unsigned char error=DYN_COMM_ERROR,i;
if(master!=0x00000000)
{
@@ -514,49 +532,30 @@ unsigned char dyn_master_is_sync_read_done(TDynamixelMaster *master,unsigned cha
// wait for the replay within the given timeout
if(master->return_level!=no_return)
{
- if(master->sync_num_dev_done<num)
+ if((error=dyn_master_is_reception_done(master))==DYN_SUCCESS)
{
- if((error=dyn_master_is_reception_done(master))==DYN_SUCCESS)
+ for(i=0;i<num;i++)
{
- if(dyn2_check_checksum(master->rx_buffer)==0x01)
+ if(dyn2_check_checksum(&master->rx_buffer[(length+11)*i])==0x01)
{
- if((error=dyn2_get_status_error(master->rx_buffer))==DYN_SUCCESS)
+ if((error=dyn2_get_status_error(&master->rx_buffer[(length+11)*i]))==DYN_NO_ERROR)
{
- while(dyn2_get_read_status_id(master->rx_buffer)!=ids[master->sync_num_dev_done]) master->sync_num_dev_done++;
- if(dyn2_get_read_status_data(master->rx_buffer,(unsigned char *)data[master->sync_num_dev_done])!=length)// not enough data
+ if(dyn2_get_read_status_data(&master->rx_buffer[(length+11)*i],(unsigned char *)data[i])!=length)// not enough data
return DYN_INST_ERROR;
- else
- {
- master->sync_num_dev_done++;
- return DYN_BUSY;
- }
}
else
- {
- master->sync_num_dev_done=0;
return error;
- }
}
else
- {
- master->sync_num_dev_done=0;
return DYN_CHECKSUM_ERROR;
- }
}
- else
- return error;
- }
- else
- {
- master->sync_num_dev_done=0;
return DYN_SUCCESS;
}
+ else
+ return error;
}
else
- {
- master->sync_num_dev_done=0;
return DYN_SUCCESS;
- }
}
else
return error;
@@ -656,7 +655,8 @@ unsigned char dyn_master_start_bulk_read(TDynamixelMaster *master,unsigned char
unsigned char dyn_master_is_bulk_read_done(TDynamixelMaster *master,unsigned char num,unsigned char *ids,unsigned short int *address, unsigned short int *length, unsigned char * const data[])
{
- unsigned char error=DYN_COMM_ERROR;
+ unsigned char error=DYN_COMM_ERROR,i;
+ unsigned short int start=0;
if(master!=0x00000000)
{
@@ -665,83 +665,51 @@ unsigned char dyn_master_is_bulk_read_done(TDynamixelMaster *master,unsigned cha
// wait for the replay within the given timeout
if(master->return_level!=no_return)
{
- if(master->bulk_num_dev_done<num)
+ if((error=dyn_master_is_reception_done(master))==DYN_SUCCESS)
{
- if((error=dyn_master_is_reception_done(master))==DYN_SUCCESS)
+ for(i=0;i<num;i++)
{
// check the input packet checksum
if(master->version==DYN_VER1)
{
- if(dyn_check_checksum(master->rx_buffer)==0xFF)
+ if(dyn_check_checksum(&master->rx_buffer[start])==0xFF)
{
- if((error=dyn_get_status_error(master->rx_buffer))==DYN_SUCCESS)
+ if((error=dyn_get_status_error(&master->rx_buffer[start]))==DYN_NO_ERROR)
{
- std::cout << "RX id: " << (int)dyn_get_read_status_id(master->rx_buffer) << " num. dev: " << (int)master->bulk_num_dev_done << std::endl;
- while(dyn_get_read_status_id(master->rx_buffer)!=ids[master->bulk_num_dev_done]) master->bulk_num_dev_done++;
- if(dyn_get_read_status_data(master->rx_buffer,(unsigned char *)data[master->bulk_num_dev_done])!=length[master->bulk_num_dev_done])// not enough data
+ if(dyn_get_read_status_data(&master->rx_buffer[start],(unsigned char *)data[i])!=length[i])// not enough data
return DYN_INST_ERROR;
- else
- {
- master->bulk_num_dev_done++;
- return DYN_BUSY;
- }
}
else
- {
- std::cout << "error" << std::endl;
- master->bulk_num_dev_done=0;
return error;
- }
}
else
- {
- std::cout << "checksum error" << std::endl;
- master->bulk_num_dev_done++;
- return DYN_BUSY;
- }
+ return DYN_CHECKSUM_ERROR;
+ start+=length[i]+6;
}
else
{
- if(dyn2_check_checksum(master->rx_buffer)==0x01)
+ if(dyn2_check_checksum(&master->rx_buffer[start])==0x01)
{
- if((error=dyn2_get_status_error(master->rx_buffer))==DYN_SUCCESS)
+ if((error=dyn2_get_status_error(&master->rx_buffer[start]))==DYN_NO_ERROR)
{
- while(dyn2_get_read_status_id(master->rx_buffer)!=ids[master->bulk_num_dev_done]) master->bulk_num_dev_done++;
- if(dyn2_get_read_status_data(master->rx_buffer,(unsigned char *)data[master->bulk_num_dev_done])!=length[master->bulk_num_dev_done])// not enough data
+ if(dyn2_get_read_status_data(&master->rx_buffer[start],(unsigned char *)data[i])!=length[i])// not enough data
return DYN_INST_ERROR;
- else
- {
- master->bulk_num_dev_done++;
- return DYN_BUSY;
- }
}
else
- {
- master->bulk_num_dev_done=0;
return error;
- }
}
else
- {
- master->bulk_num_dev_done++;
- return DYN_BUSY;
- }
+ return DYN_CHECKSUM_ERROR;
+ start+=length[i]+11;
}
}
- else
- return error;
- }
- else
- {
- master->bulk_num_dev_done=0;
return DYN_SUCCESS;
}
+ else
+ return error;
}
else
- {
- master->bulk_num_dev_done=0;
return DYN_SUCCESS;
- }
}
else
return error;
@@ -770,8 +738,7 @@ void dyn_master_init(TDynamixelMaster *master,TComm *dev,TDynVersion version)
master->rx_no_answer=0x00;
master->rx_num_packets=0x00;
master->return_level=return_all;
- master->sync_num_dev_done=0;
- master->bulk_num_dev_done=0;
+ master->packet_bytes=0;
master->set_rx_mode();
}
@@ -822,7 +789,23 @@ unsigned char dyn_master_ping(TDynamixelMaster *master,unsigned char id)
return error;
}
// wait for the replay within the given timeout
- error=dyn_master_wait_reception(master);
+ if((error=dyn_master_wait_reception(master))!=DYN_SUCCESS)
+ return error;
+ // check the input packet checksum
+ if(master->version==DYN_VER1)
+ {
+ if(dyn_check_checksum(master->rx_buffer)==0xFF)
+ return dyn_get_status_error(master->rx_buffer);
+ else
+ return DYN_CHECKSUM_ERROR;
+ }
+ else
+ {
+ if(dyn2_check_checksum(master->rx_buffer)==0x01)
+ return dyn2_get_status_error(master->rx_buffer);
+ else
+ return DYN_CHECKSUM_ERROR;
+ }
}
return error;
@@ -880,15 +863,36 @@ unsigned char dyn_master_read_table(TDynamixelMaster *master,unsigned char id,un
{
if((error=dyn_master_wait_reception(master))==DYN_SUCCESS)
{
+ // check the input packet checksum
if(master->version==DYN_VER1)
{
- if(dyn_get_read_status_data(master->rx_buffer,data)!=length)// not enough data
- error=DYN_INST_ERROR;
+ if(dyn_check_checksum(master->rx_buffer)==0xFF)
+ {
+ if((error=dyn_get_status_error(master->rx_buffer))==DYN_NO_ERROR)
+ {
+ if(dyn_get_read_status_data(master->rx_buffer,data)!=length)// not enough data
+ return DYN_INST_ERROR;
+ }
+ else
+ return error;
+ }
+ else
+ return DYN_CHECKSUM_ERROR;
}
else
{
- if(dyn2_get_read_status_data(master->rx_buffer,data)!=length)// not enough data
- error=DYN_INST_ERROR;
+ if(dyn2_check_checksum(master->rx_buffer)==0x01)
+ {
+ if((error=dyn2_get_status_error(master->rx_buffer))==DYN_NO_ERROR)
+ {
+ if(dyn2_get_read_status_data(master->rx_buffer,data)!=length)// not enough data
+ return DYN_INST_ERROR;
+ }
+ else
+ return error;
+ }
+ else
+ return DYN_CHECKSUM_ERROR;
}
}
}
@@ -943,7 +947,26 @@ unsigned char dyn_master_write_table(TDynamixelMaster *master,unsigned char id,
}
// wait for the replay within the given timeout
if(master->return_level==return_all && id!=DYN_BROADCAST_ID)
- error=dyn_master_wait_reception(master);
+ {
+ if((error=dyn_master_wait_reception(master))==DYN_SUCCESS)
+ {
+ // check the input packet checksum
+ if(master->version==DYN_VER1)
+ {
+ if(dyn_check_checksum(master->rx_buffer)==0xFF)
+ error=dyn_get_status_error(master->rx_buffer);
+ else
+ error=DYN_CHECKSUM_ERROR;
+ }
+ else
+ {
+ if(dyn2_check_checksum(master->rx_buffer)==0x01)
+ error=dyn2_get_status_error(master->rx_buffer);
+ else
+ error=DYN_CHECKSUM_ERROR;
+ }
+ }
+ }
}
return error;
@@ -981,7 +1004,26 @@ unsigned char dyn_master_reg_write(TDynamixelMaster *master,unsigned char id, un
}
// wait for the replay within the given timeout
if(master->return_level==return_all && id!=DYN_BROADCAST_ID)
- error=dyn_master_wait_reception(master);
+ {
+ if((error=dyn_master_wait_reception(master))==DYN_SUCCESS)
+ {
+ // check the input packet checksum
+ if(master->version==DYN_VER1)
+ {
+ if(dyn_check_checksum(master->rx_buffer)==0xFF)
+ error=dyn_get_status_error(master->rx_buffer);
+ else
+ error=DYN_CHECKSUM_ERROR;
+ }
+ else
+ {
+ if(dyn2_check_checksum(master->rx_buffer)==0x01)
+ error=dyn2_get_status_error(master->rx_buffer);
+ else
+ error=DYN_CHECKSUM_ERROR;
+ }
+ }
+ }
}
return error;
@@ -1083,13 +1125,22 @@ unsigned char dyn_master_sync_read(TDynamixelMaster *master,unsigned char num,un
// wait for the replay within the given timeout
if(master->return_level!=no_return)
{
- for(i=0;i<num;i++)
+ if((error=dyn_master_wait_reception(master))==DYN_SUCCESS)
{
- if((error=dyn_master_wait_reception(master))==DYN_SUCCESS)
+ for(i=0;i<num;i++)
{
- while(dyn2_get_read_status_id(master->rx_buffer)!=ids[i]) i++;
- if(dyn2_get_read_status_data(master->rx_buffer,(unsigned char *)data[i])!=length)// not enough data
- error=DYN_INST_ERROR;
+ if(dyn2_check_checksum(&master->rx_buffer[(length+11)*i])==0x01)
+ {
+ if((error=dyn2_get_status_error(&master->rx_buffer[(length+11)*i]))==DYN_NO_ERROR)
+ {
+ if(dyn2_get_read_status_data(&master->rx_buffer[(length+11)*i],(unsigned char *)data[i])!=length)// not enough data
+ return DYN_INST_ERROR;
+ }
+ else
+ return error;
+ }
+ else
+ return DYN_CHECKSUM_ERROR;
}
}
}
@@ -1102,6 +1153,7 @@ unsigned char dyn_master_sync_read(TDynamixelMaster *master,unsigned char num,un
unsigned char dyn_master_bulk_read(TDynamixelMaster *master,unsigned char num,unsigned char *ids,unsigned short int *address, unsigned short int *length, unsigned char * const data[])
{
unsigned char error=DYN_COMM_ERROR,i,ver1_address[255],ver1_length[255];
+ unsigned short int start=0;
if(master!=0x00000000)
{
@@ -1139,21 +1191,42 @@ unsigned char dyn_master_bulk_read(TDynamixelMaster *master,unsigned char num,un
// wait for the replay within the given timeout
if(master->return_level!=no_return)
{
- for(i=0;i<num;i++)
+ if((error=dyn_master_wait_reception(master))==DYN_SUCCESS)
{
- if((error=dyn_master_wait_reception(master))==DYN_SUCCESS)
+ for(i=0;i<num;i++)
{
+ // check the input packet checksum
if(master->version==DYN_VER1)
{
- while(dyn_get_read_status_id(master->rx_buffer)!=ids[i]) i++;
- if(dyn_get_read_status_data(master->rx_buffer,(unsigned char *)data[i])!=length[i])// not enough data
- error=DYN_INST_ERROR;
+ if(dyn_check_checksum(&master->rx_buffer[start])==0xFF)
+ {
+ if((error=dyn_get_status_error(&master->rx_buffer[start]))==DYN_NO_ERROR)
+ {
+ if(dyn_get_read_status_data(&master->rx_buffer[start],(unsigned char *)data[i])!=length[i])// not enough data
+ return DYN_INST_ERROR;
+ }
+ else
+ return error;
+ }
+ else
+ return DYN_CHECKSUM_ERROR;
+ start+=length[i]+6;
}
else
{
- while(dyn2_get_read_status_id(master->rx_buffer)!=ids[i]) i++;
- if(dyn2_get_read_status_data(master->rx_buffer,(unsigned char *)data[i])!=length[i])// not enough data
- error=DYN_INST_ERROR;
+ if(dyn2_check_checksum(&master->rx_buffer[start])==0x01)
+ {
+ if((error=dyn2_get_status_error(&master->rx_buffer[start]))==DYN_NO_ERROR)
+ {
+ if(dyn2_get_read_status_data(&master->rx_buffer[start],(unsigned char *)data[i])!=length[i])// not enough data
+ return DYN_INST_ERROR;
+ }
+ else
+ return error;
+ }
+ else
+ return DYN_CHECKSUM_ERROR;
+ start+=length[i]+11;
}
}
}
diff --git a/dynamixel_manager/include/dyn_manager.h b/dynamixel_manager/include/dyn_manager.h
index e408c8fd3d392468784a75c60cb51ad2168a835c..e82ffc5d997d5613a44af1666b5e4bc31c2f4f7b 100644
--- a/dynamixel_manager/include/dyn_manager.h
+++ b/dynamixel_manager/include/dyn_manager.h
@@ -127,6 +127,10 @@ typedef struct{
unsigned char stop_flag;
unsigned int present_devices;
volatile unsigned char lock;
+ unsigned char modules_period_count[DYN_MANAGER_MAX_NUM_MODULES];
+ unsigned char single_op_period_count[DYN_MANAGER_MAX_NUM_SINGLE_OP];
+ unsigned char sync_op_period_count[DYN_MANAGER_MAX_NUM_SYNC_OP];
+ unsigned char bulk_op_period_count[DYN_MANAGER_MAX_NUM_BULK_OP];
}TDynManager;
// public functions
diff --git a/dynamixel_manager/src/dyn_manager.c b/dynamixel_manager/src/dyn_manager.c
index 59076d3b0623d4dd71bd1033c571b76a09604af0..b9282ff446e165f93de35bf79334bbae8d3aa43a 100644
--- a/dynamixel_manager/src/dyn_manager.c
+++ b/dynamixel_manager/src/dyn_manager.c
@@ -253,6 +253,7 @@ unsigned char dyn_manager_init(TDynManager *manager,TMemory *memory,TScheduler *
manager->operations[i].single_op[j].address=0x0000;
manager->operations[i].single_op[j].length=0x0000;
manager->operations[i].single_op[j].data=0x00000000;
+ manager->single_op_period_count[i]=0;
}
manager->operations[i].num_single_op=0;
for(j=0;j<DYN_MANAGER_MAX_NUM_SYNC_OP;j++)
@@ -269,6 +270,7 @@ unsigned char dyn_manager_init(TDynManager *manager,TMemory *memory,TScheduler *
}
manager->operations[i].sync_op[j].address=0x0000;
manager->operations[i].sync_op[j].length=0x0000;
+ manager->sync_op_period_count[i]=0;
}
manager->operations[i].num_sync_op=0;
for(j=0;j<DYN_MANAGER_MAX_NUM_BULK_OP;j++)
@@ -285,12 +287,16 @@ unsigned char dyn_manager_init(TDynManager *manager,TMemory *memory,TScheduler *
manager->operations[i].bulk_op[j].length[k]=0x0000;
manager->operations[i].bulk_op[j].data[k]=0x00000000;
}
+ manager->bulk_op_period_count[i]=0;
}
manager->operations[i].num_bulk_op=0;
}
manager->num_masters=0;
for(i=0;i<DYN_MANAGER_MAX_NUM_MODULES;i++)
+ {
manager->modules[i]=0x00000000;
+ manager->modules_period_count[i]=0;
+ }
manager->num_modules=0;
for(i=0;i<DYN_MANAGER_MAX_NUM_DEVICES;i++)
{
@@ -659,7 +665,7 @@ OP_HANDLE *dyn_manager_sync_op_new(TDynManager *manager,unsigned char mode,unsig
current_op->length=length;
current_op->ids[current_op->num_devices]=ids[i];
current_op->data[current_op->num_devices]=data[i];
- current_op->num_devices++;
+ current_op->num_devices=1;
manager->operations[master_index].num_sync_op++;
}
}
@@ -689,18 +695,21 @@ OP_HANDLE *dyn_manager_sync_op_new(TDynManager *manager,unsigned char mode,unsig
void dyn_manager_sync_op_add_devices(TDynManager *manager,OP_HANDLE *op,unsigned char num,unsigned char *ids,unsigned char * const data[])
{
- TDynManagerSyncOp *current_op;
- unsigned char i,j;
+ TDynManagerSyncOp *current_op,*other_op;
+ unsigned char i,j,master_index,current_op_index;
dyn_manager_lock(manager);
- for(i=0;i<manager->num_masters;i++)
+ for(i=0;i<num;i++)
{
- if(op->op_index[i]!=0xFF)
+ if((master_index=dyn_manager_check_id(manager,ids[i]))==0xFF)
+ continue;
+ else
{
- current_op=&manager->operations[i].sync_op[op->op_index[i]];
- for(j=0;j<num;j++)
+ if(op->op_index[master_index]!=0xFF)
{
- if(dyn_manager_id_present(current_op->num_devices,current_op->ids,ids[j])==0x00)
+ current_op_index=op->op_index[master_index];
+ current_op=&manager->operations[master_index].sync_op[current_op_index];
+ if(dyn_manager_id_present(current_op->num_devices,current_op->ids,ids[i])==0x00)
{
if(current_op->num_devices<DYN_MANAGER_MAX_NUM_DEVICES)
{
@@ -710,6 +719,30 @@ void dyn_manager_sync_op_add_devices(TDynManager *manager,OP_HANDLE *op,unsigned
}
}
}
+ else
+ {
+ current_op_index=manager->operations[master_index].num_sync_op;
+ if(current_op_index<DYN_MANAGER_MAX_NUM_SYNC_OP)
+ {
+ current_op=&manager->operations[master_index].sync_op[current_op_index];
+ for(j=0;j<DYN_MANAGER_MAX_NUM_MASTERS;j++)
+ if(op->op_index[j]!=0xFF)
+ {
+ other_op=&manager->operations[j].sync_op[current_op_index];
+ current_op->address=other_op->address;
+ current_op->length=other_op->length;
+ current_op->common.operation=other_op->common.operation;
+ current_op->common.repetitions=other_op->common.repetitions;
+ current_op->common.period_count=other_op->common.period_count;
+ current_op->common.op_handle=other_op->common.op_handle;
+ }
+ op->op_index[master_index]=current_op_index;
+ current_op->ids[current_op->num_devices]=ids[i];
+ current_op->data[current_op->num_devices]=data[i];
+ current_op->num_devices=1;
+ manager->operations[master_index].num_sync_op++;
+ }
+ }
}
}
dyn_manager_unlock(manager);
@@ -875,29 +908,54 @@ OP_HANDLE *dyn_manager_bulk_op_new(TDynManager *manager,unsigned char mode,unsig
void dyn_manager_bulk_op_add_devices(TDynManager *manager,OP_HANDLE *op,unsigned char num,unsigned char *ids,unsigned short int *address, unsigned short int *length,unsigned char * const data[])
{
- TDynManagerBulkOp *current_op;
- unsigned char i,j;
+ TDynManagerBulkOp *current_op,*other_op;
+ unsigned char i,j,master_index,current_op_index;
dyn_manager_lock(manager);
- for(i=0;i<manager->num_masters;i++)
+ for(i=0;i<num;i++)
{
- if(op->op_index[i]!=0xFF)
+ if((master_index=dyn_manager_check_id(manager,ids[i]))==0xFF)
+ continue;
+ else
{
- current_op=&manager->operations[i].bulk_op[op->op_index[i]];
- for(j=0;j<num;j++)
+ if(op->op_index[master_index]!=0xFF)
{
- if(dyn_manager_id_present(current_op->num_devices,current_op->ids,ids[j])==0x00)
+ current_op_index=op->op_index[master_index];
+ current_op=&manager->operations[master_index].bulk_op[current_op_index];
+ if(dyn_manager_id_present(current_op->num_devices,current_op->ids,ids[i])==0x00)
{
if(current_op->num_devices<DYN_MANAGER_MAX_NUM_DEVICES)
{
current_op->ids[current_op->num_devices]=ids[i];
- current_op->address[current_op->num_devices]=address[i];
- current_op->length[current_op->num_devices]=length[i];
current_op->data[current_op->num_devices]=data[i];
current_op->num_devices++;
}
}
}
+ else
+ {
+ current_op_index=manager->operations[master_index].num_bulk_op;
+ if(current_op_index<DYN_MANAGER_MAX_NUM_SYNC_OP)
+ {
+ current_op=&manager->operations[master_index].bulk_op[current_op_index];
+ for(j=0;j<DYN_MANAGER_MAX_NUM_MASTERS;j++)
+ if(op->op_index[j]!=0xFF)
+ {
+ other_op=&manager->operations[j].bulk_op[current_op_index];
+ current_op->common.operation=other_op->common.operation;
+ current_op->common.repetitions=other_op->common.repetitions;
+ current_op->common.period_count=other_op->common.period_count;
+ current_op->common.op_handle=other_op->common.op_handle;
+ }
+ op->op_index[master_index]=current_op_index;
+ current_op->ids[current_op->num_devices]=ids[i];
+ current_op->data[current_op->num_devices]=data[i];
+ current_op->address[current_op->num_devices]=address[i];
+ current_op->length[current_op->num_devices]=length[i];
+ current_op->num_devices=1;
+ manager->operations[master_index].num_bulk_op++;
+ }
+ }
}
}
dyn_manager_unlock(manager);
@@ -1018,12 +1076,9 @@ void dyn_manager_loop(TDynManager *manager)
TDynManagerSingleOp *single_op[DYN_MANAGER_MAX_NUM_MASTERS];
TDynManagerSyncOp *sync_op[DYN_MANAGER_MAX_NUM_MASTERS];
TDynManagerBulkOp *bulk_op[DYN_MANAGER_MAX_NUM_MASTERS];
- static unsigned char modules_period_count[DYN_MANAGER_MAX_NUM_MODULES]={0};
- static unsigned char single_op_period_count[DYN_MANAGER_MAX_NUM_SINGLE_OP]={0};
- static unsigned char sync_op_period_count[DYN_MANAGER_MAX_NUM_SINGLE_OP]={0};
- static unsigned char bulk_op_period_count[DYN_MANAGER_MAX_NUM_SINGLE_OP]={0};
// initialize the remaining operations
+ dyn_manager_lock(manager);
for(i=0;i<manager->num_masters;i++)
{
states[i]=start_single_ops;
@@ -1037,19 +1092,17 @@ void dyn_manager_loop(TDynManager *manager)
{
if(manager->modules[i]->enabled)
{
- modules_period_count[i]++;
- if(modules_period_count[i]==manager->modules[i]->period_count)
+ manager->modules_period_count[i]++;
+ if(manager->modules_period_count[i]==manager->modules[i]->period_count)
{
if(manager->modules[i]->pre_process!=0x00000000)
manager->modules[i]->pre_process(manager->modules[i]->data);
- modules_period_count[i]=0;
+ manager->modules_period_count[i]=0;
}
}
}
// send all the commands
- std::cout << "start loop" << std::endl;
- dyn_manager_lock(manager);
do{
for(i=0;i<manager->num_masters;i++)
{
@@ -1059,16 +1112,16 @@ void dyn_manager_loop(TDynManager *manager)
states[i]=start_bulk_ops;
else
{
- single_op_period_count[current_single_op[i]]++;
+ manager->single_op_period_count[current_single_op[i]]++;
single_op[i]=&manager->operations[i].single_op[current_single_op[i]];
- if(single_op_period_count[current_single_op[i]]==single_op[i]->common.period_count)
+ if(manager->single_op_period_count[current_single_op[i]]==single_op[i]->common.period_count)
{
if((single_op[i]->common.operation&DYN_MANAGER_MODE_MASK)==DYN_MANAGER_READ)
dyn_master_start_read_table(manager->masters[i],single_op[i]->id,single_op[i]->address,single_op[i]->length,single_op[i]->data);
else
dyn_master_start_write_table(manager->masters[i],single_op[i]->id,single_op[i]->address,single_op[i]->length,single_op[i]->data);
states[i]=wait_single_ops;
- single_op_period_count[current_single_op[i]]=0;
+ manager->single_op_period_count[current_single_op[i]]=0;
}
else
{
@@ -1101,17 +1154,16 @@ void dyn_manager_loop(TDynManager *manager)
}
else
{
- sync_op_period_count[current_sync_op[i]]++;
+ manager->sync_op_period_count[current_sync_op[i]]++;
sync_op[i]=&manager->operations[i].sync_op[current_sync_op[i]];
- if(sync_op_period_count[current_sync_op[i]]==sync_op[i]->common.period_count)
+ if(manager->sync_op_period_count[current_sync_op[i]]==sync_op[i]->common.period_count)
{
if((sync_op[i]->common.operation&DYN_MANAGER_MODE_MASK)==DYN_MANAGER_READ)
dyn_master_start_sync_read(manager->masters[i],sync_op[i]->num_devices,sync_op[i]->ids,sync_op[i]->address,sync_op[i]->length,sync_op[i]->data);
else
dyn_master_start_sync_write(manager->masters[i],sync_op[i]->num_devices,sync_op[i]->ids,sync_op[i]->address,sync_op[i]->length,sync_op[i]->data);
- usleep(300);
states[i]=wait_sync_ops;
- sync_op_period_count[current_sync_op[i]]=0;
+ manager->sync_op_period_count[current_sync_op[i]]=0;
}
else
{
@@ -1141,17 +1193,16 @@ void dyn_manager_loop(TDynManager *manager)
states[i]=start_sync_ops;
else
{
- bulk_op_period_count[current_bulk_op[i]]++;
+ manager->bulk_op_period_count[current_bulk_op[i]]++;
bulk_op[i]=&manager->operations[i].bulk_op[current_bulk_op[i]];
- if(bulk_op_period_count[current_bulk_op[i]]==bulk_op[i]->common.period_count)
+ if(manager->bulk_op_period_count[current_bulk_op[i]]==bulk_op[i]->common.period_count)
{
if((bulk_op[i]->common.operation&DYN_MANAGER_MODE_MASK)==DYN_MANAGER_READ)
dyn_master_start_bulk_read(manager->masters[i],bulk_op[i]->num_devices,bulk_op[i]->ids,bulk_op[i]->address,bulk_op[i]->length,bulk_op[i]->data);
else
dyn_master_start_bulk_write(manager->masters[i],bulk_op[i]->num_devices,bulk_op[i]->ids,bulk_op[i]->address,bulk_op[i]->length,bulk_op[i]->data);
- usleep(300);
states[i]=wait_bulk_ops;
- bulk_op_period_count[current_bulk_op[i]]=0;
+ manager->bulk_op_period_count[current_bulk_op[i]]=0;
}
else
{
@@ -1182,22 +1233,21 @@ void dyn_manager_loop(TDynManager *manager)
}
}
}while(loop_done>0);
- dyn_manager_unlock(manager);
- std::cout << "loop done" << std::endl;
// execute the pre_process functions for all modules
for(i=0;i<manager->num_modules;i++)
{
if(manager->modules[i]->enabled)
{
- if(modules_period_count[i]==manager->modules[i]->period_count)
+ if(manager->modules_period_count[i]==manager->modules[i]->period_count)
{
if(manager->modules[i]->post_process!=0x00000000)
manager->modules[i]->post_process(manager->modules[i]->data);
- modules_period_count[i]=0;
+ manager->modules_period_count[i]=0;
}
}
}
+ dyn_manager_unlock(manager);
if(manager->stop_flag==0x01)
{
manager->running=0x00;
diff --git a/dynamixel_manager/src/dyn_module.c b/dynamixel_manager/src/dyn_module.c
index 949fe66af2017bf01bc7ed029dfadd6953da41f3..5f52cb2634ebe3cf9d955b2f85a43d1e2dd20544 100644
--- a/dynamixel_manager/src/dyn_module.c
+++ b/dynamixel_manager/src/dyn_module.c
@@ -76,9 +76,12 @@ void dyn_module_add_device(TDynModule *module,unsigned char id, unsigned short i
for(i=0;i<module->num_models;i++)
{
if(model==module->models[i])
- {
- module->assigned_ids[id]=0x01;
- module->num_assigned_ids++;
+ {
+ if(module->assigned_ids[id]==0x00)
+ {
+ module->assigned_ids[id]=0x01;
+ module->num_assigned_ids++;
+ }
if(module->add_device!=0x00000000)
module->add_device(module->data,id,model);
break;
diff --git a/dynamixel_manager/src/modules/motion_manager.c b/dynamixel_manager/src/modules/motion_manager.c
index c542a68b79322ca9970d1ea22e9df4ffdb71b81b..cfc7a2d0f697c4a92de0c327a1d9b39937b6ec0a 100644
--- a/dynamixel_manager/src/modules/motion_manager.c
+++ b/dynamixel_manager/src/modules/motion_manager.c
@@ -135,8 +135,8 @@ void mmanager_add_device(TMotionManager *mmanager,unsigned char id,unsigned shor
if(id<DYN_MANAGER_MAX_NUM_DEVICES)
{
for(i=0;i<NUM_SERVO_MODELS;i++)
- if(model==servo_data[i].model)
- mmanager->servo_configs[id]=&servo_data[i];
+ if(model==servo_data[i].model)
+ mmanager->servo_configs[id]=&servo_data[i];
mmanager_read_register(mmanager,id,&mmanager->servo_configs[id]->registers[current_pos],&mmanager->servo_values[id].current_value);
mmanager_read_register(mmanager,id,&mmanager->servo_configs[id]->registers[min_angle_limit],&cw_value);
@@ -366,17 +366,20 @@ void mmanager_enable_servo(TMotionManager *mmanager,unsigned char servo_id)
if(servo_id<DYN_MANAGER_MAX_NUM_DEVICES)
{
- mmanager->servo_values[servo_id].enabled=0x01;
- /* add an operation to enable the servo */
- if(mmanager->servo_configs[servo_id]!=0x00000000)
+ if(mmanager->servo_values[servo_id].enabled==0x00)
{
- if(mmanager->enable_op==0x00000000 || mmanager->enable_op->op_type==no_op)
+ mmanager->servo_values[servo_id].enabled=0x01;
+ /* add an operation to enable the servo */
+ if(mmanager->servo_configs[servo_id]!=0x00000000)
{
- mmanager->enable_op=dyn_manager_sync_op_new(mmanager->dyn_module.manager,DYN_MANAGER_WRITE,1,&servo_id,mmanager->servo_configs[servo_id]->registers[torque_en].address,1,&data);
- dyn_manager_set_op_repetitions(mmanager->dyn_module.manager,mmanager->enable_op,1);
+ if(mmanager->enable_op==0x00000000 || mmanager->enable_op->op_type==no_op)
+ {
+ mmanager->enable_op=dyn_manager_sync_op_new(mmanager->dyn_module.manager,DYN_MANAGER_WRITE,1,&servo_id,mmanager->servo_configs[servo_id]->registers[torque_en].address,1,&data);
+ dyn_manager_set_op_repetitions(mmanager->dyn_module.manager,mmanager->enable_op,1);
+ }
+ else
+ dyn_manager_sync_op_add_devices(mmanager->dyn_module.manager,mmanager->enable_op,1,&servo_id,&data);
}
- else
- dyn_manager_sync_op_add_devices(mmanager->dyn_module.manager,mmanager->enable_op,1,&servo_id,&data);
}
}
}
@@ -387,17 +390,20 @@ void mmanager_disable_servo(TMotionManager *mmanager,unsigned char servo_id)
if(servo_id<DYN_MANAGER_MAX_NUM_DEVICES)
{
- mmanager->servo_values[servo_id].enabled=0x00;
- /* add an operation to enable the servo */
- if(mmanager->servo_configs[servo_id]!=0x00000000)
+ if(mmanager->servo_values[servo_id].enabled==0x01)
{
- if(mmanager->enable_op==0x00000000 || mmanager->enable_op->op_type==no_op)
+ mmanager->servo_values[servo_id].enabled=0x00;
+ /* add an operation to enable the servo */
+ if(mmanager->servo_configs[servo_id]!=0x00000000)
{
- mmanager->enable_op=dyn_manager_sync_op_new(mmanager->dyn_module.manager,DYN_MANAGER_WRITE,1,&servo_id,mmanager->servo_configs[servo_id]->registers[torque_en].address,1,&data);
- dyn_manager_set_op_repetitions(mmanager->dyn_module.manager,mmanager->enable_op,1);
- }
- else
- dyn_manager_sync_op_add_devices(mmanager->dyn_module.manager,mmanager->enable_op,1,&servo_id,&data);
+ if(mmanager->enable_op==0x00000000 || mmanager->enable_op->op_type==no_op)
+ {
+ mmanager->enable_op=dyn_manager_sync_op_new(mmanager->dyn_module.manager,DYN_MANAGER_WRITE,1,&servo_id,mmanager->servo_configs[servo_id]->registers[torque_en].address,1,&data);
+ dyn_manager_set_op_repetitions(mmanager->dyn_module.manager,mmanager->enable_op,1);
+ }
+ else
+ dyn_manager_sync_op_add_devices(mmanager->dyn_module.manager,mmanager->enable_op,1,&servo_id,&data);
+ }
}
}
}
diff --git a/scheduler/include/scheduler.h b/scheduler/include/scheduler.h
index 40b835104285c1c424fd69d12353fc00fb5b77f0..69c5399691d50890e9ca5acec2932dd08e569044 100644
--- a/scheduler/include/scheduler.h
+++ b/scheduler/include/scheduler.h
@@ -45,6 +45,8 @@ typedef struct{
void (* set_pulse)(unsigned short int,unsigned short int,unsigned char);
}TScheduler;
+unsigned int scheduler_get_channel(sched_channel_t channel_id);
+sched_channel_t scheduler_get_id(unsigned int id);
void scheduler_init(TScheduler *scheduler,unsigned char num_channels, unsigned short int prescaler);
void scheduler_interrupt(TScheduler *scheduler,sched_channel_t channel_id);
unsigned short int scheduler_get_pulse(TScheduler *scheduler, sched_channel_t channel_id);
diff --git a/scheduler/src/scheduler.c b/scheduler/src/scheduler.c
index a7501520384d9cd7af5c2fb34c72ddd4f947052c..9dcbb906092429142011cbc77263272f86cb8dfe 100644
--- a/scheduler/src/scheduler.c
+++ b/scheduler/src/scheduler.c
@@ -20,6 +20,18 @@ unsigned int scheduler_get_channel(sched_channel_t channel_id)
}
}
+sched_channel_t scheduler_get_id(unsigned int id)
+{
+ switch(id)
+ {
+ case TIM_CHANNEL_1: return SCHED_CH1;
+ case TIM_CHANNEL_2: return SCHED_CH2;
+ case TIM_CHANNEL_3: return SCHED_CH3;
+ case TIM_CHANNEL_4: return SCHED_CH4;
+ default: return SCHED_CH1;
+ }
+}
+
void scheduler_init(TScheduler *scheduler,unsigned char num_channels, unsigned short int prescaler)
{
unsigned char i=0;