From f0f8d07fd9d457c5fc03ef3abb4120098316a22a Mon Sep 17 00:00:00 2001
From: Sergi Hernandez Juan <shernand@iri.upc.edu>
Date: Wed, 29 Jan 2020 00:22:37 +0100
Subject: [PATCH] Solved a bug in the dynamixel manager: the period of the
modules was not properly updated. Solved a bug in the motion manager: write
data is stored in RAM. Problems with muliple calls to the setup function():
still not solved.
---
dynamixel_manager/src/dyn_manager.c | 4 +++-
dynamixel_manager/src/dyn_module.c | 2 +-
dynamixel_manager/src/modules/motion_manager.c | 13 +++++++++++++
dynamixel_manager/src/modules/motion_pages.c | 2 +-
4 files changed, 18 insertions(+), 3 deletions(-)
diff --git a/dynamixel_manager/src/dyn_manager.c b/dynamixel_manager/src/dyn_manager.c
index db25cf8..63e7ee5 100644
--- a/dynamixel_manager/src/dyn_manager.c
+++ b/dynamixel_manager/src/dyn_manager.c
@@ -351,6 +351,7 @@ void dyn_manager_scan(TDynManager *manager)
unsigned short int model;
manager->present_devices=0x00000000;
+ manager->num_devices=0;
// scan all dynamixel buses
for(i=0;i<manager->num_masters;i++)
{
@@ -939,7 +940,7 @@ 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_MASTERS]={0};
+ 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};
@@ -973,6 +974,7 @@ void dyn_manager_loop(TDynManager *manager)
{
if(manager->modules[i]->pre_process!=0x00000000)
manager->modules[i]->pre_process(manager->modules[i]->data);
+ modules_period_count[i]=0;
}
}
}
diff --git a/dynamixel_manager/src/dyn_module.c b/dynamixel_manager/src/dyn_module.c
index 342b4ac..949fe66 100644
--- a/dynamixel_manager/src/dyn_module.c
+++ b/dynamixel_manager/src/dyn_module.c
@@ -30,7 +30,7 @@ unsigned char dyn_module_init(TDynModule *module,TMemory *memory,unsigned short
module->assigned_ids[i]=0x00;
module->num_assigned_ids=0x00;
module->period_count=0x00;
- module->enabled=0x00;
+ module->enabled=0x01;
module->add_device=0x00000000;
module->set_period=0x00000000;
module->setup=0x00000000;
diff --git a/dynamixel_manager/src/modules/motion_manager.c b/dynamixel_manager/src/modules/motion_manager.c
index 84dcad8..3e5a676 100644
--- a/dynamixel_manager/src/modules/motion_manager.c
+++ b/dynamixel_manager/src/modules/motion_manager.c
@@ -65,6 +65,7 @@ void mmanager_write_cmd(void *module,unsigned short int address,unsigned short i
}
}
}
+ ram_write_table(manager->memory,address,length,data);
}
unsigned short int mmanager_angle_to_value(TMotionManager *mmanager,unsigned char servo_id,short int angle);
@@ -171,7 +172,11 @@ void mmanager_setup(TMotionManager *mmanager)
}
}
if(num>0)
+ {
+ if(mmanager->motion_op[0]!=0x00000000)
+ dyn_manager_delete_op(mmanager->dyn_module.manager,mmanager->motion_op[0]);
mmanager->motion_op[0]=dyn_manager_sync_op_new(mmanager->dyn_module.manager,DYN_MANAGER_WRITE,num,ids,start_address,4,(unsigned char * const*)&data);
+ }
num=0;
for(i=0;i<DYN_MANAGER_MAX_NUM_DEVICES;i++)
{
@@ -191,7 +196,11 @@ void mmanager_setup(TMotionManager *mmanager)
}
}
if(num>0)
+ {
+ if(mmanager->motion_op[1]!=0x00000000)
+ dyn_manager_delete_op(mmanager->dyn_module.manager,mmanager->motion_op[1]);
mmanager->motion_op[1]=dyn_manager_sync_op_new(mmanager->dyn_module.manager,DYN_MANAGER_WRITE,num,ids,start_address,2,(unsigned char * const*)&data);
+ }
/* create a feedback operation with the servos that support bulk read */
num=0;
for(i=0;i<DYN_MANAGER_MAX_NUM_DEVICES;i++)
@@ -209,7 +218,11 @@ void mmanager_setup(TMotionManager *mmanager)
}
}
if(num>0)
+ {
+ if(mmanager->feedback_op[0]!=0x00000000)
+ dyn_manager_delete_op(mmanager->dyn_module.manager,mmanager->feedback_op[0]);
mmanager->feedback_op[0]=dyn_manager_bulk_op_new(mmanager->dyn_module.manager,DYN_MANAGER_READ,num,ids,address,length,(unsigned char * const*)data);
+ }
}
void mmanager_set_period(TMotionManager *mmanager,unsigned short period_ms)
diff --git a/dynamixel_manager/src/modules/motion_pages.c b/dynamixel_manager/src/modules/motion_pages.c
index bf4f7d8..34bec5e 100755
--- a/dynamixel_manager/src/modules/motion_pages.c
+++ b/dynamixel_manager/src/modules/motion_pages.c
@@ -42,4 +42,4 @@ inline unsigned char pages_get_slope(TPage *page,unsigned char servo_id)
return 0x01<<(page->header.slope[servo_id]&0x0F);
}
-TPage motion_pages[MAX_PAGES] __attribute__ ((section (".pages"))) __attribute__((weak)) ;
+TPage motion_pages[MAX_PAGES] __attribute__ ((section (".pages"))) __attribute__((weak));
--
GitLab