diff --git a/dynamixel_base/src/dynamixel_master.c b/dynamixel_base/src/dynamixel_master.c index 14163f064f88487c74f9ff8fe8ba2cd9fadde2af..d615cd33caa170c8972e207280b4e0206a880761 100644 --- a/dynamixel_base/src/dynamixel_master.c +++ b/dynamixel_base/src/dynamixel_master.c @@ -14,14 +14,23 @@ void dyn_master_receive_cb(TDynamixelMaster *master) if(master->rx_num_packets==0x00) { time_cancel_timeout(&master->time); + master->sync_bulk_address=0x0000; master->packet_ready=0x01; } else { if(master->version==DYN_VER1) + { + if(master->rx_num_packets==0x01) + master->sync_bulk_address=0x0000; master->rx_irq(master->hal_dev,&master->rx_buffer[master->sync_bulk_address],4); + } else + { + if(master->rx_num_packets==0x01) + master->sync_bulk_address=0x0000; master->rx_irq(master->hal_dev,&master->rx_buffer[master->sync_bulk_address],7); + } } } else @@ -35,6 +44,7 @@ void dyn_master_receive_cb(TDynamixelMaster *master) { length=master->rx_buffer[master->sync_bulk_address+3]; master->rx_dma(master->hal_dev,&master->rx_buffer[master->sync_bulk_address+4],length); + master->sync_bulk_address+=length+4; } } else @@ -46,9 +56,9 @@ void dyn_master_receive_cb(TDynamixelMaster *master) { length=master->rx_buffer[master->sync_bulk_address+5]+(master->rx_buffer[master->sync_bulk_address+6]<<8); master->rx_dma(master->hal_dev,&master->rx_buffer[master->sync_bulk_address+7],length); + master->sync_bulk_address+=length+7; } } - master->sync_bulk_address+=length; } } } @@ -57,7 +67,6 @@ void dyn_master_send_cb(TDynamixelMaster *master) { if(master!=0x00000000) { - master->tx_done=0x01; master->set_rx_mode(); if(master->rx_no_answer) master->rx_no_answer=0x00; @@ -66,8 +75,11 @@ void dyn_master_send_cb(TDynamixelMaster *master) if(master->version==DYN_VER1) master->rx_irq(master->hal_dev,master->rx_buffer,4); else + { master->rx_irq(master->hal_dev,master->rx_buffer,7); + } } + master->tx_done=0x01; } } @@ -164,6 +176,7 @@ unsigned char dyn_master_send(TDynamixelMaster *master) if((error=dyn_master_wait_transmission(master))!=DYN_SUCCESS) return error; // set the DMA transfer + master->tx_done=0x00; if(master->version==DYN_VER1) { master->tx_dma(master->hal_dev,master->tx_buffer,dyn_get_length(master->tx_buffer)+4); @@ -456,7 +469,6 @@ unsigned char dyn_master_start_sync_read(TDynamixelMaster *master,unsigned char master->rx_no_answer=0x00; // enable transmission master->set_tx_mode(); - master->sync_bulk_address=0x0000; // send the data if((error=dyn_master_send(master))!=DYN_SUCCESS) master->set_rx_mode(); @@ -594,7 +606,6 @@ unsigned char dyn_master_start_bulk_read(TDynamixelMaster *master,unsigned char master->rx_no_answer=0x00; // enable transmission master->set_tx_mode(); - master->sync_bulk_address=0x0000; // send the data if((error=dyn_master_send(master))!=DYN_SUCCESS) master->set_rx_mode(); @@ -700,7 +711,7 @@ void dyn_master_init(TDynamixelMaster *master,void *hal_dev,TDynVersion version) master->rx_no_answer=0x00; master->rx_num_packets=0x00; master->return_level=return_all; - master->sync_bulk_address; + master->sync_bulk_address=0x0000; master->set_rx_mode(); } @@ -1073,7 +1084,6 @@ unsigned char dyn_master_sync_read(TDynamixelMaster *master,unsigned char num,un master->rx_no_answer=0x00; // enable transmission master->set_tx_mode(); - master->sync_bulk_address=0x0000; // send the data if((error=dyn_master_send(master))!=DYN_SUCCESS) { @@ -1140,7 +1150,6 @@ unsigned char dyn_master_bulk_read(TDynamixelMaster *master,unsigned char num,un master->rx_no_answer=0x00; // enable transmission master->set_tx_mode(); - master->sync_bulk_address=0x0000; // send the data if((error=dyn_master_send(master))!=DYN_SUCCESS) {