From a682f15c55fb59da9bc87aba516f29337937a40b Mon Sep 17 00:00:00 2001
From: Sergi Hernandez Juan <shernand@iri.upc.edu>
Date: Thu, 30 Jul 2020 15:46:39 +0200
Subject: [PATCH 1/6] Changed the servo angles in the motion pages data type to
unsigned type. The action module cast the value to a signed type.
---
dynamixel_manager/include/modules/motion_pages.h | 2 +-
1 file changed, 1 insertion(+), 1 deletion(-)
diff --git a/dynamixel_manager/include/modules/motion_pages.h b/dynamixel_manager/include/modules/motion_pages.h
index 56f8feb..9edefcb 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;
--
GitLab
From ae2588e822eeb9e4424b0730821f08d24d194efb Mon Sep 17 00:00:00 2001
From: Sergi Hernandez Juan <shernand@iri.upc.edu>
Date: Thu, 30 Jul 2020 15:48:09 +0200
Subject: [PATCH 2/6] The action module casts the servo angle data to signed
type.
---
dynamixel_manager/src/modules/action.c | 6 +++---
1 file changed, 3 insertions(+), 3 deletions(-)
diff --git a/dynamixel_manager/src/modules/action.c b/dynamixel_manager/src/modules/action.c
index 4720389..c21dd68 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
--
GitLab
From 9b2d34e15dd176b0432533b3ab790c465665688a Mon Sep 17 00:00:00 2001
From: Sergi Hernandez Juan <shernand@iri.upc.edu>
Date: Wed, 2 Sep 2020 12:59:17 +0200
Subject: [PATCH 3/6] Solved some bugs in the protocol version conversion
functions.
---
dynamixel_base/src/dynamixel.c | 23 +++++++++++++++++------
dynamixel_base/src/dynamixel2.c | 26 ++++++++++++++++++++------
2 files changed, 37 insertions(+), 12 deletions(-)
diff --git a/dynamixel_base/src/dynamixel.c b/dynamixel_base/src/dynamixel.c
index e25fdb7..8c00138 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 dc26232..ce34b11 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;
--
GitLab
From 042183c31d57807fecce6af795e8335a2a3434e6 Mon Sep 17 00:00:00 2001
From: Sergi Hernandez Juan <shernand@iri.upc.edu>
Date: Wed, 2 Sep 2020 13:00:21 +0200
Subject: [PATCH 4/6] Improved the dyn_master_relay function with internal
protocol version conversion.
---
dynamixel_base/include/dynamixel_master.h | 2 +-
dynamixel_base/src/dynamixel_master.c | 34 ++++++++++++++++++++---
2 files changed, 31 insertions(+), 5 deletions(-)
diff --git a/dynamixel_base/include/dynamixel_master.h b/dynamixel_base/include/dynamixel_master.h
index 70fd70d..64df2a8 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_master.c b/dynamixel_base/src/dynamixel_master.c
index a91820b..0fb6a96 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;
--
GitLab
From 6840c1091628c4e8967ff203aaecf05a1e674865 Mon Sep 17 00:00:00 2001
From: Sergi Hernandez Juan <shernand@iri.upc.edu>
Date: Wed, 2 Sep 2020 13:01:06 +0200
Subject: [PATCH 5/6] The loop functions return 0xFF to indicate the packet has
to be relayed (not processed or broadcast ID)
---
dynamixel_base/src/dynamixel_slave_device.c | 12 ++++++++++++
1 file changed, 12 insertions(+)
diff --git a/dynamixel_base/src/dynamixel_slave_device.c b/dynamixel_base/src/dynamixel_slave_device.c
index 3d792dc..68c1222 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;
}
}
--
GitLab
From 188e1f8bda9d26ef613378a93087ce29d528c755 Mon Sep 17 00:00:00 2001
From: Sergi Hernandez Juan <shernand@iri.upc.edu>
Date: Wed, 2 Sep 2020 13:02:03 +0200
Subject: [PATCH 6/6] Improved and tested the relay feature.
---
dynamixel_base/src/dynamixel_slave.c | 20 +++++++++++++-------
1 file changed, 13 insertions(+), 7 deletions(-)
diff --git a/dynamixel_base/src/dynamixel_slave.c b/dynamixel_base/src/dynamixel_slave.c
index a08f9b0..84aaeae 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
{
--
GitLab