diff --git a/dynamixel_base/include/dynamixel_master.h b/dynamixel_base/include/dynamixel_master.h
index 70fd70dd96dd7b9a96ddd3fd877a9ad4916e3d9b..64df2a8516c1973238b95bf8921815cd8e94939b 100644
--- a/dynamixel_base/include/dynamixel_master.h
+++ b/dynamixel_base/include/dynamixel_master.h
@@ -214,7 +214,7 @@ unsigned char dyn_master_bulk_write(TDynamixelMaster *master,unsigned char num,u
* \brief
*
*/
-unsigned char dyn_master_relay(TDynamixelMaster *master,unsigned char *inst_pkt,unsigned char *status_pkt);
+unsigned char dyn_master_relay(TDynamixelMaster *master,TDynVersion inst_ver,unsigned char *inst_pkt,unsigned char *status_pkt);
#ifdef __cplusplus
}
diff --git a/dynamixel_base/src/dynamixel.c b/dynamixel_base/src/dynamixel.c
index e25fdb7d54f046e2f9c10587e61bebd062860c46..8c00138cf8697fb3304a1d00874292afa0fb4109 100755
--- a/dynamixel_base/src/dynamixel.c
+++ b/dynamixel_base/src/dynamixel.c
@@ -51,10 +51,21 @@ unsigned char dyn_convert_v2_inst_packet(unsigned char *source, unsigned char *d
destination[DYN_HEADER_OFF]=0xFF;
destination[DYN_HEADER_OFF+1]=0xFF;
destination[DYN_ID_OFF]=source[DYN2_ID_OFF];
- destination[DYN_LENGTH_OFF]=source[DYN2_LENGTH_OFF];
+ destination[DYN_LENGTH_OFF]=2;//source[DYN2_LENGTH_OFF];
destination[DYN_INST_OFF]=source[DYN2_INST_OFF];
- for(i=0;i<destination[DYN_LENGTH_OFF];i++)
- destination[DYN_DATA_OFF+i]=source[DYN2_DATA_OFF+i];
+ switch(source[DYN2_INST_OFF])
+ {
+ case DYN_PING: break;
+ case DYN_READ: destination[DYN_DATA_OFF]=source[DYN2_DATA_OFF];
+ destination[DYN_DATA_OFF+1]=source[DYN2_DATA_OFF+2];
+ destination[DYN_LENGTH_OFF]+=2;
+ break;
+ case DYN_WRITE: destination[DYN_DATA_OFF]=source[DYN2_DATA_OFF];
+ for(i=0;i<source[DYN2_DATA_OFF]-5;i++)
+ destination[DYN_DATA_OFF+1+i]=source[DYN2_DATA_OFF+2+i];
+ destination[DYN_LENGTH_OFF]+=source[DYN2_DATA_OFF]-4;
+ break;
+ }
dyn_set_checksum(destination);
return 0x01;
}
@@ -247,10 +258,10 @@ unsigned char dyn_convert_v2_status_packet(unsigned char *source, unsigned char
destination[DYN_HEADER_OFF]=0xFF;
destination[DYN_HEADER_OFF+1]=0xFF;
destination[DYN_ID_OFF]=source[DYN2_ID_OFF];
- destination[DYN_LENGTH_OFF]=source[DYN2_LENGTH_OFF];
+ destination[DYN_LENGTH_OFF]=source[DYN2_LENGTH_OFF]-2;
destination[DYN_ERROR_OFF]=source[DYN2_ERROR_OFF];
- for(i=0;i<destination[DYN_LENGTH_OFF];i++)
- destination[DYN_DATA_OFF+i]=source[DYN2_DATA_OFF+i];
+ for(i=0;i<destination[DYN_LENGTH_OFF]-2;i++)
+ destination[DYN_DATA_OFF+i]=source[DYN2_DATA_OFF+1+i];
dyn_set_checksum(destination);
return 0x01;
}
diff --git a/dynamixel_base/src/dynamixel2.c b/dynamixel_base/src/dynamixel2.c
index dc26232184df175583bddc9fa84c80a00e06d7a5..ce34b1154a505a3db5824391ce6a288b857a49b0 100644
--- a/dynamixel_base/src/dynamixel2.c
+++ b/dynamixel_base/src/dynamixel2.c
@@ -88,11 +88,25 @@ unsigned char dyn2_convert_v1_inst_packet(unsigned char *source, unsigned char *
destination[DYN2_HEADER_OFF+2]=0xFD;
destination[DYN2_RESERVED]=0x00;
destination[DYN2_ID_OFF]=source[DYN_ID_OFF];
- destination[DYN2_LENGTH_OFF]=source[DYN_LENGTH_OFF];
+ destination[DYN2_LENGTH_OFF]=3;//source[DYN_LENGTH_OFF];
destination[DYN2_LENGTH_OFF+1]=0x00;
destination[DYN2_INST_OFF]=source[DYN_INST_OFF];
- for(i=0;i<source[DYN_LENGTH_OFF];i++)
- destination[DYN2_DATA_OFF+i]=source[DYN_DATA_OFF+i];
+ switch(source[DYN_INST_OFF])
+ {
+ case DYN_PING: break;
+ case DYN_READ: destination[DYN2_DATA_OFF]=source[DYN_DATA_OFF];
+ destination[DYN2_DATA_OFF+1]=0x00;
+ destination[DYN2_DATA_OFF+2]=source[DYN_DATA_OFF+1];
+ destination[DYN2_DATA_OFF+3]=0x00;
+ (*((unsigned short int *)&destination[DYN2_LENGTH_OFF]))+=4;
+ break;
+ case DYN_WRITE: destination[DYN2_DATA_OFF]=source[DYN_DATA_OFF];
+ destination[DYN2_DATA_OFF+1]=0x00;
+ for(i=0;i<source[DYN_LENGTH_OFF]-3;i++)
+ destination[DYN2_DATA_OFF+2+i]=source[DYN_DATA_OFF+1+i];
+ (*((unsigned short int *)&destination[DYN2_LENGTH_OFF]))+=source[DYN_LENGTH_OFF]-1;
+ break;
+ }
dyn2_set_checksum(destination);
return 0x01;
@@ -417,12 +431,12 @@ unsigned char dyn2_convert_v1_status_packet(TDynInstruction inst,unsigned char *
destination[DYN2_HEADER_OFF+2]=0xFD;
destination[DYN2_RESERVED]=0x00;
destination[DYN2_ID_OFF]=source[DYN_ID_OFF];
- destination[DYN2_LENGTH_OFF]=source[DYN_LENGTH_OFF];
+ destination[DYN2_LENGTH_OFF]=source[DYN_LENGTH_OFF]+2;
destination[DYN2_LENGTH_OFF+1]=0x00;
destination[DYN2_INST_OFF]=inst;
destination[DYN2_ERROR_OFF]=source[DYN_ERROR_OFF];
- for(i=0;i<source[DYN_LENGTH_OFF];i++)
- destination[DYN2_DATA_OFF+i]=source[DYN_DATA_OFF+i];
+ for(i=0;i<destination[DYN2_LENGTH_OFF]-4;i++)
+ destination[DYN2_DATA_OFF+1+i]=source[DYN_DATA_OFF+i];
dyn2_set_checksum(destination);
return 0x01;
diff --git a/dynamixel_base/src/dynamixel_master.c b/dynamixel_base/src/dynamixel_master.c
index a91820b2c85dee635a94337860956ec78d5335e6..0fb6a964ca20c8823c3b6e3c997771e3e763238a 100644
--- a/dynamixel_base/src/dynamixel_master.c
+++ b/dynamixel_base/src/dynamixel_master.c
@@ -1297,13 +1297,26 @@ unsigned char dyn_master_bulk_write(TDynamixelMaster *master,unsigned char num,u
return error;
}
-unsigned char dyn_master_relay(TDynamixelMaster *master,unsigned char *inst_pkt,unsigned char *status_pkt)
+unsigned char dyn_master_relay(TDynamixelMaster *master,TDynVersion inst_ver,unsigned char *inst_pkt,unsigned char *status_pkt)
{
- unsigned char error=DYN_COMM_ERROR;
+ unsigned char error=DYN_COMM_ERROR,i;
if(master!=0x00000000)
{
- dyn2_copy_packet(inst_pkt,master->tx_buffer);
+ if(inst_ver==DYN_VER1)
+ {
+ if(master->version==DYN_VER1)
+ dyn_copy_packet(inst_pkt,master->tx_buffer);
+ else
+ dyn2_convert_v1_inst_packet(inst_pkt,master->tx_buffer);
+ }
+ else
+ {
+ if(master->version==DYN_VER1)
+ dyn_convert_v2_inst_packet(inst_pkt,master->tx_buffer);
+ else
+ dyn2_copy_packet(inst_pkt,master->tx_buffer);
+ }
master->rx_num_packets=0x01;
master->rx_no_answer=0x00;
// enable transmission
@@ -1322,7 +1335,20 @@ unsigned char dyn_master_relay(TDynamixelMaster *master,unsigned char *inst_pkt,
}
// wait for the replay within the given timeout
error=dyn_master_wait_reception(master);
- dyn2_copy_packet(master->rx_buffer,status_pkt);
+ if(inst_ver==DYN_VER1)
+ {
+ if(master->version==DYN_VER1)
+ dyn_copy_packet(master->rx_buffer,status_pkt);
+ else
+ dyn_convert_v2_status_packet(master->rx_buffer,status_pkt);
+ }
+ else
+ {
+ if(master->version==DYN_VER1)
+ dyn2_convert_v1_status_packet((TDynInstruction)inst_pkt[DYN2_INST_OFF],master->rx_buffer,status_pkt);
+ else
+ dyn2_copy_packet(master->rx_buffer,status_pkt);
+ }
}
return error;
diff --git a/dynamixel_base/src/dynamixel_slave.c b/dynamixel_base/src/dynamixel_slave.c
index a08f9b063ef5bcf7173cb8a218865c8f3e611b0a..84aaeaeabccf810a950c7e9488d07a76d042f302 100644
--- a/dynamixel_base/src/dynamixel_slave.c
+++ b/dynamixel_base/src/dynamixel_slave.c
@@ -169,13 +169,19 @@ void dyn_slave_loop(TDynamixelSlave *slave)
if(send_status==0x01)
dyn_slave_send_status_packet(slave,slave->slave_devices[i]->address,error,length,data,slave->slave_devices[i]->return_delay);
}
- if(slave->on_relay(slave->version,slave->rx_buffer,slave->tx_buffer)==DYN_SUCCESS)
- {
- // set the tx mode, if necessary
- slave->set_tx_mode();
- // start transmission by DMA
- comm_send_dma(slave->comm_dev,slave->tx_buffer,dyn_get_length(slave->tx_buffer)+4);
- }
+ if(send_status==0xFF)// packet has not been processed
+ {
+ if(slave->on_relay(slave->version,slave->rx_buffer,slave->tx_buffer)==DYN_SUCCESS)
+ {
+ // set the tx mode, if necessary
+ slave->set_tx_mode();
+ // start transmission by DMA
+ if(slave->version==DYN_VER1)
+ comm_send_dma(slave->comm_dev,slave->tx_buffer,dyn_get_length(slave->tx_buffer)+4);
+ else
+ comm_send_dma(slave->comm_dev,slave->tx_buffer,dyn2_get_length(slave->tx_buffer)+7);
+ }
+ }
}
else
{
diff --git a/dynamixel_base/src/dynamixel_slave_device.c b/dynamixel_base/src/dynamixel_slave_device.c
index 3d792dc043937e39ce47630f8ed16f8b8c868ee4..68c122281b01a4b3d0f9fbc6f5770e6ea8346c21 100644
--- a/dynamixel_base/src/dynamixel_slave_device.c
+++ b/dynamixel_base/src/dynamixel_slave_device.c
@@ -98,6 +98,8 @@ unsigned char dyn_v1_slave_loop(TDynamixelSlaveDevice *device,unsigned char *rx_
(*error)=DYN_INST_ERROR;
}
}
+ else if(id==DYN_BROADCAST_ID)
+ send_status=0xFF;
break;
case DYN_WRITE: (*length)=dyn_get_write_data(rx_buffer,data);
(*error)=device->on_write(dyn_get_write_address(rx_buffer),*length,data);
@@ -108,6 +110,8 @@ unsigned char dyn_v1_slave_loop(TDynamixelSlaveDevice *device,unsigned char *rx_
if((*error)!=DYN_NO_ERROR)
(*error)=DYN_INST_ERROR;
}
+ else if(id==DYN_BROADCAST_ID)
+ send_status=0xFF;
break;
case DYN_REG_WRITE: device->reg_length=dyn_get_reg_write_data(rx_buffer,device->reg_buffer);
device->reg_address=dyn_get_reg_write_address(rx_buffer);
@@ -199,6 +203,8 @@ unsigned char dyn_v1_slave_loop(TDynamixelSlaveDevice *device,unsigned char *rx_
}
}
}
+ else
+ send_status=0xFF;
}
}
@@ -242,6 +248,8 @@ unsigned char dyn_v2_slave_loop(TDynamixelSlaveDevice *device,unsigned char *rx_
(*error)=DYN_INST_ERROR;
}
}
+ else if(id==DYN_BROADCAST_ID)
+ send_status=0xFF;
break;
case DYN_WRITE: (*length)=dyn2_get_write_data(rx_buffer,data);
(*error)=device->on_write(dyn2_get_write_address(rx_buffer),*length,data);
@@ -252,6 +260,8 @@ unsigned char dyn_v2_slave_loop(TDynamixelSlaveDevice *device,unsigned char *rx_
if((*error)!=DYN_NO_ERROR)
(*error)=DYN_INST_ERROR;
}
+ else if(id==DYN_BROADCAST_ID)
+ send_status=0xFF;
break;
case DYN_REG_WRITE: device->reg_length=dyn2_get_reg_write_data(rx_buffer,device->reg_buffer);
device->reg_address=dyn2_get_reg_write_address(rx_buffer);
@@ -376,6 +386,8 @@ unsigned char dyn_v2_slave_loop(TDynamixelSlaveDevice *device,unsigned char *rx_
}
}
}
+ else
+ send_status=0xFF;
}
}
diff --git a/dynamixel_manager/include/modules/motion_pages.h b/dynamixel_manager/include/modules/motion_pages.h
index 56f8febe2818fe91a67ca43c686cd058da8567ca..9edefcb230b1028f4364ae76de7ac0f4a534b177 100755
--- a/dynamixel_manager/include/modules/motion_pages.h
+++ b/dynamixel_manager/include/modules/motion_pages.h
@@ -35,7 +35,7 @@ typedef struct // Header Structure (total 64unsigned char)
typedef struct // Step Structure (total 64unsigned char)
{
- short int position[PAGE_MAX_NUM_SERVOS]; // Joint position 0~61
+ unsigned short int position[PAGE_MAX_NUM_SERVOS]; // Joint position 0~61
unsigned char pause; // Pause time 62
unsigned char time; // Time 63
}TStep;
diff --git a/dynamixel_manager/src/modules/action.c b/dynamixel_manager/src/modules/action.c
index 47203899fd870bb6c03732e5ff30afdcda2a2eb2..c21dd68abb78820670255982f4eae6763fa6b916 100755
--- a/dynamixel_manager/src/modules/action.c
+++ b/dynamixel_manager/src/modules/action.c
@@ -120,7 +120,7 @@ void action_load_next_step(TActionMModule *action)
{
if(mmanager_get_module(action->mmodule.manager,i)==MM_ACTION)
{
- angle=action->current_page.steps[action->current_step_index].position[i];
+ angle=(signed short int)action->current_page.steps[action->current_step_index].position[i];
if(angle==0x5A00)// bigger than 180
target_angles=action->mmodule.manager->servo_values[i].target_angle;
else
@@ -131,9 +131,9 @@ void action_load_next_step(TActionMModule *action)
else
{
if(action->current_step_index==action->current_page.header.stepnum-1)
- next_angle=action->next_page.steps[0].position[i];
+ next_angle=(signed short int)action->next_page.steps[0].position[i];
else
- next_angle=action->current_page.steps[action->current_step_index+1].position[i];
+ next_angle=(signed short int)action->current_page.steps[action->current_step_index+1].position[i];
if(next_angle==0x5A00)
next_angles=target_angles;
else