diff --git a/dynamixel_manager/include/dyn_manager.h b/dynamixel_manager/include/dyn_manager.h
index 58cbc68136ee42181371e28dc62c69768c049a7a..834faa5ee09ad0cc45f76bc83cd3a8492f27e075 100644
--- a/dynamixel_manager/include/dyn_manager.h
+++ b/dynamixel_manager/include/dyn_manager.h
@@ -122,6 +122,8 @@ typedef struct{
sched_channel_t sch_channel;
TMemory *memory;
TMemModule mem_module;
+ unsigned char running;
+ unsigned char do_scan;
}TDynManager;
// public functions
@@ -131,6 +133,8 @@ static inline unsigned char dyn_manager_get_period(TDynManager *manager)
{
return manager->period_ms;
}
+void dyn_manager_start(TDynManager *manager);
+void dyn_manager_stop(TDynManager *manager);
void dyn_manager_scan(TDynManager *manager);
void dyn_manager_add_master(TDynManager *manager,TDynamixelMaster *master);
static inline unsigned char dyn_manager_get_num_masters(TDynManager *manager)
diff --git a/dynamixel_manager/include/dyn_manager_registers.h b/dynamixel_manager/include/dyn_manager_registers.h
index 70940a8207dd39b2d6986d733f15f9d941666747..af8c3c629298e053e80e7413feb9b847f52f551c 100644
--- a/dynamixel_manager/include/dyn_manager_registers.h
+++ b/dynamixel_manager/include/dyn_manager_registers.h
@@ -1,17 +1,28 @@
#ifndef _DYN_MANAGER_REGISTERS_H
#define _DYN_MANAGER_REGISTERS_H
-#define RAM_DYN_MANAGER_LENGTH 2
+#define RAM_DYN_MANAGER_LENGTH 4
+
+#define DYN_MANAGER_NUM_MODULES_OFFSET 0
+#define DYN_MANAGER_NUM_MASTERS_OFFSET 1
+#define DYN_MANAGER_CONTROL_OFFSET 2 // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
+ // scanning | running | | | | start scan | stop | start
+#define DYN_MANAGER_START 0x01
+#define DYN_MANAGER_STOP 0x02
+#define DYN_MANAGER_START_SCAN 0x04
+#define DYN_MANAGER_RUNNING 0x40
+#define DYN_MANAGER_SCANNING 0x80
+
+#define DYN_MANAGER_NUM_DEVICES_OFFSET 3
#define EEPROM_DYN_MANAGER_LENGTH 1
#define DYN_MANAGER_PERIOD_OFFSET 0
-#define DYN_MANAGER_NUM_MODULES_OFFSET 0
-#define DYN_MANAGER_NUM_MASTERS_OFFSET 1
-
#define dyn_manager_eeprom_data(name,section_name,base_address,DEFAULT_PERIOD) \
-unsigned short int name##_eeprom_data[] __attribute__ ((section (section_name)))={DEFAULT_PERIOD&0x00FF,base_address+DYN_MANAGER_PERIOD};
+unsigned short int name##_eeprom_data[] __attribute__ ((section (section_name))) __attribute__ ((aligned (4)))={ \
+ DEFAULT_PERIOD&0x00FF,base_address+DYN_MANAGER_PERIOD_OFFSET \
+};
#endif
diff --git a/dynamixel_manager/include/dyn_module_registers.h b/dynamixel_manager/include/dyn_module_registers.h
index 0269bbecbf8665f8d115641befcc5ddc333918c2..4e1fc4477f7062f637eaf3f45850ef8670de8266 100644
--- a/dynamixel_manager/include/dyn_module_registers.h
+++ b/dynamixel_manager/include/dyn_module_registers.h
@@ -3,15 +3,17 @@
#define RAM_DYN_MODULE_LENGTH 2
+#define DYN_MODULE_NUM_MODELS_OFFSET 0
+#define DYN_MODULE_NUM_DEVICES_OFFSET 1
+
#define EEPROM_DYN_MODULE_LENGTH 1
#define DYN_MODULE_PERIOD_OFFSET 0
-#define DYN_MODULE_NUM_MODELS_OFFSET 0
-#define DYN_MODULE_NUM_DEVICES_OFFSET 1
-
#define dyn_module_eeprom_data(name,section_name,base_address,DEFAULT_PERIOD) \
-unsigned short int name##_eeprom_data[] __attribute__ ((section (section_name)))={DEFAULT_PERIOD,base_address+DYN_MODULE_PERIOD};
+unsigned short int name##_eeprom_data[] __attribute__ ((section (section_name))) __attribute__ ((aligned (4)))={ \
+ DEFAULT_PERIOD,base_address+DYN_MODULE_PERIOD_OFFSET \
+};
#endif
diff --git a/dynamixel_manager/include/modules/motion_manager.h b/dynamixel_manager/include/modules/motion_manager.h
index 6f09e0c42ebe6e1f1fd0a6528c5f5de40ab523a3..f2ac07d48ef91af79de0a148ffb7fcffa2448877 100644
--- a/dynamixel_manager/include/modules/motion_manager.h
+++ b/dynamixel_manager/include/modules/motion_manager.h
@@ -7,6 +7,7 @@ extern "C" {
#include "dyn_module.h"
#include "dyn_servos.h"
+#include "motion_manager_registers.h"
#ifndef MM_MAX_NUM_MOTION_MODULES
#error "Please, specify the maximum number of motion modules with the MM_MAX_NUM_MOTION_MODULES macro"
diff --git a/dynamixel_manager/include/modules/motion_manager_registers.h b/dynamixel_manager/include/modules/motion_manager_registers.h
index 3fa14283eabbd27209c92d2bc0b5e67c5c0d6b1e..6086f1169749c53d878a9d73787beb3c87e933c9 100644
--- a/dynamixel_manager/include/modules/motion_manager_registers.h
+++ b/dynamixel_manager/include/modules/motion_manager_registers.h
@@ -6,14 +6,20 @@
#define RAM_MM_LENGTH (DYN_MANAGER_MAX_NUM_DEVICES/2)
#define MM_ENABLE_MODULE_OFFSET 0
+ #define MM_EVEN_SER_EN 0x80
+ #define MM_EVEN_SER_MOD 0x70
+ #define MM_ODD_SER_EN 0x08
+ #define MM_ODD_SER_MOD 0x07
#define EEPROM_MM_LENGTH (DYN_MANAGER_MAX_NUM_DEVICES)
#define MM_SERVO_OFFSET_OFFSET 0
-#define dyn_mm_eeprom_data(name,section_name,base_address,DEFAULT_PERIOD,initial_offset) \
-dyn_module_eeprom_data(name,section_name,base_address,DEFAULT_PERIOD) \
-unsigned short int name##_eeprom_data[] __attribute__ ((section (section_name)))={initial_offset};
+#define dyn_mm_eeprom_data(name,section_name,base_address,DEFAULT_PERIOD,...) \
+dyn_module_eeprom_data(name##_module,section_name,base_address,DEFAULT_PERIOD) \
+unsigned short int name##_eeprom_data[] __attribute__ ((section (section_name))) __attribute__ ((aligned (4)))={ \
+ __VA_ARGS__ \
+};
#endif
diff --git a/dynamixel_manager/src/dyn_manager.c b/dynamixel_manager/src/dyn_manager.c
index 9d6ef84a5453a0dd74e9da81c64b3d810fa1bf37..3eac3989c6840fc6202df469d7c15f466b4c3ef8 100644
--- a/dynamixel_manager/src/dyn_manager.c
+++ b/dynamixel_manager/src/dyn_manager.c
@@ -18,14 +18,32 @@ extern unsigned char dyn_master_is_bulk_read_done(TDynamixelMaster *master,unsig
typedef enum {start_single_ops,wait_single_ops,start_sync_ops,wait_sync_ops,start_bulk_ops,wait_bulk_ops,ops_done} dyn_manager_states_t;
// private functions
+void dyn_manager_start_scan(TDynManager *manager)
+{
+ if(manager->running==0x00)
+ dyn_manager_start(manager);
+ manager->do_scan=0x01;
+ manager->memory->data[manager->ram_base_address+DYN_MANAGER_CONTROL_OFFSET]|=DYN_MANAGER_SCANNING;
+}
+
void dyn_manager_write_cmd(TDynManager *module,unsigned short int address,unsigned short int length,unsigned char *data)
{
- unsigned short int eeprom_offset;
+ unsigned short int eeprom_offset,ram_offset;
eeprom_offset=address-module->eeprom_base_address;
if(ram_in_window(module->eeprom_base_address+DYN_MANAGER_PERIOD_OFFSET,1,address,length))
dyn_manager_set_period(module,data[DYN_MANAGER_PERIOD_OFFSET-eeprom_offset]);
- ram_write_table(module->memory,address,length,data);
+
+ ram_offset=address-module->ram_base_address;
+ if(ram_in_range(module->ram_base_address+DYN_MANAGER_CONTROL_OFFSET,address,length))
+ {
+ if(data[DYN_MANAGER_CONTROL_OFFSET-ram_offset]&DYN_MANAGER_START)
+ dyn_manager_start(module);
+ if(data[DYN_MANAGER_CONTROL_OFFSET-ram_offset]&DYN_MANAGER_STOP)
+ dyn_manager_stop(module);
+ if(data[DYN_MANAGER_CONTROL_OFFSET-ram_offset]&DYN_MANAGER_START_SCAN)
+ dyn_manager_start_scan(module);
+ }
}
void dyn_manager_read_cmd(TDynManager *module,unsigned short int address,unsigned short int length,unsigned char *data)
@@ -204,7 +222,7 @@ void dyn_manager_delete_op_single_master(TDynManager *manager,unsigned char mast
}
// public functions
-unsigned char dyn_manager_init(TDynManager *manager,TMemory *memory,TScheduler *scheduler,sched_channel_t sch_channel,unsigned short int eeprom_base_address,unsigned short int ram_base_address)
+unsigned char dyn_manager_init(TDynManager *manager,TMemory *memory,TScheduler *scheduler,sched_channel_t sch_channel,unsigned short int ram_base_address,unsigned short int eeprom_base_address)
{
unsigned char i,j,k;
@@ -293,6 +311,8 @@ unsigned char dyn_manager_init(TDynManager *manager,TMemory *memory,TScheduler *
manager->eeprom_base_address=eeprom_base_address;
manager->ram_base_address=ram_base_address;
manager->memory=memory;
+ manager->running=0x00;
+ manager->do_scan=0x00;
return 0x01;
}
@@ -306,6 +326,21 @@ void dyn_manager_set_period(TDynManager *manager,unsigned char period_ms)
if(manager->modules[i]!=0x00000000 && manager->modules[i]->set_period!=0x00000000)
manager->modules[i]->set_period(manager->modules[i]->data,period_ms*manager->modules[i]->period_count);
manager->period_ms=period_ms;
+ manager->memory->data[manager->eeprom_base_address+DYN_MANAGER_PERIOD_OFFSET]=period_ms;
+}
+
+void dyn_manager_start(TDynManager *manager)
+{
+ scheduler_enable_channel(manager->scheduler,manager->sch_channel);
+ manager->running=0x01;
+ manager->memory->data[manager->ram_base_address+DYN_MANAGER_CONTROL_OFFSET]|=DYN_MANAGER_RUNNING;
+}
+
+void dyn_manager_stop(TDynManager *manager)
+{
+ scheduler_disable_channel(manager->scheduler,manager->sch_channel);
+ manager->running=0x00;
+ manager->memory->data[manager->ram_base_address+DYN_MANAGER_CONTROL_OFFSET]&=(~DYN_MANAGER_RUNNING);
}
void dyn_manager_scan(TDynManager *manager)
@@ -337,6 +372,8 @@ void dyn_manager_scan(TDynManager *manager)
// else ignore device
}
}
+ manager->memory->data[manager->ram_base_address+DYN_MANAGER_NUM_DEVICES_OFFSET]=manager->num_devices;
+ manager->memory->data[manager->ram_base_address+DYN_MANAGER_CONTROL_OFFSET]&=(~DYN_MANAGER_SCANNING);
// configure the operations
for(i=0;i<manager->num_modules;i++)
if(manager->modules[i]->setup!=0x00000000)
@@ -900,6 +937,16 @@ void dyn_manager_loop(TDynManager *manager)
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};
+ // do scan
+ if(manager->do_scan)
+ {
+ manager->do_scan=0x00;
+// dyn_manager_stop(manager);
+ dyn_manager_scan(manager);
+// dyn_manager_start(manager);
+ return;
+ }
+
// initialize the remaining operations
for(i=0;i<manager->num_masters;i++)
{
diff --git a/dynamixel_manager/src/dyn_module.c b/dynamixel_manager/src/dyn_module.c
index a5c470e85970c1b9fdf16d7aa872392fd9507cbc..342b4ac7c8c565035f38ff99743059651899b2fd 100644
--- a/dynamixel_manager/src/dyn_module.c
+++ b/dynamixel_manager/src/dyn_module.c
@@ -13,7 +13,7 @@ void dyn_module_write_cmd(TDynModule *module,unsigned short int address,unsigned
ram_write_table(module->memory,address,length,data);
}
-void dyn_module_read_cmd(TDynManager *module,unsigned short int address,unsigned short int length,unsigned char *data)
+void dyn_module_read_cmd(TDynModule *module,unsigned short int address,unsigned short int length,unsigned char *data)
{
ram_read_table(module->memory,address,length,data);
}
diff --git a/dynamixel_manager/src/modules/motion_manager.c b/dynamixel_manager/src/modules/motion_manager.c
index 07fa4b0156b49ec5a31073e806cb4a366e0e86ba..a5fc87b8df43a6ef1eda1622799a1c8a01cee241 100644
--- a/dynamixel_manager/src/modules/motion_manager.c
+++ b/dynamixel_manager/src/modules/motion_manager.c
@@ -1,6 +1,7 @@
#include "motion_manager.h"
#include "motion_manager_registers.h"
#include "motion_module.h"
+#include "ram.h"
/* private functions */
void mmanager_read_register(TMotionManager *mmanager,unsigned char id,TDynReg *reg,unsigned int *value)
@@ -25,12 +26,45 @@ void mmanager_write_register(TMotionManager *mmanager,unsigned char id,TDynReg *
void mmanager_read_cmd(void *module,unsigned short int address,unsigned short int length,unsigned char *data)
{
+ TMotionManager *manager=(TMotionManager *)module;
+ ram_read_table(manager->memory,address,length,data);
}
void mmanager_write_cmd(void *module,unsigned short int address,unsigned short int length,unsigned char *data)
{
+ TMotionManager *manager=(TMotionManager *)module;
+ unsigned short int eeprom_offset,ram_offset,i,j;
+ unsigned char byte_value,mod;
+ eeprom_offset=address-manager->eeprom_base_address;
+ if(ram_in_window(manager->eeprom_base_address+MM_SERVO_OFFSET_OFFSET,DYN_MANAGER_MAX_NUM_DEVICES,address,length))
+ {
+ for(i=0;i<DYN_MANAGER_MAX_NUM_DEVICES;i++)
+ {
+ if(ram_in_range(manager->eeprom_base_address+MM_SERVO_OFFSET_OFFSET+i,address,length))
+ mmanager_set_offset(manager,i,(signed char)(data[MM_SERVO_OFFSET_OFFSET-eeprom_offset+i]));
+ }
+ }
+ ram_offset=address-manager->ram_base_address;
+ if(ram_in_window(manager->ram_base_address+MM_ENABLE_MODULE_OFFSET,DYN_MANAGER_MAX_NUM_DEVICES/2,address,length))
+ {
+ for(i=0,j=0;i<DYN_MANAGER_MAX_NUM_DEVICES/2;i++,j+=2)
+ {
+ if(ram_in_range(manager->ram_base_address+MM_ENABLE_MODULE_OFFSET+i,address,length))
+ {
+ byte_value=data[MM_ENABLE_MODULE_OFFSET-ram_offset+i];
+ if(byte_value&MM_EVEN_SER_EN) mmanager_enable_servo(manager,j);
+ else mmanager_disable_servo(manager,j);
+ mod=(byte_value&MM_EVEN_SER_MOD)>>4;
+ mmanager_set_module(manager,j,(TModules)mod);
+ if(byte_value&MM_ODD_SER_EN) mmanager_enable_servo(manager,j+1);
+ else mmanager_disable_servo(manager,j+1);
+ mod=byte_value&MM_ODD_SER_MOD;
+ mmanager_set_module(manager,j+1,(TModules)mod);
+ }
+ }
+ }
}
unsigned short int mmanager_angle_to_value(TMotionManager *mmanager,unsigned char servo_id,short int angle);
@@ -124,7 +158,7 @@ void mmanager_setup(TMotionManager *mmanager)
{
first=0;
if(mmanager->servo_configs[i]->pid==1)
- start_address=mmanager->servo_configs[i]->registers[pos_pid_i].address;
+ start_address=mmanager->servo_configs[i]->registers[pos_pid_p].address;
else
start_address=mmanager->servo_configs[i]->registers[cw_comp_slope].address;
}
@@ -169,7 +203,8 @@ void mmanager_setup(TMotionManager *mmanager)
}
}
}
- mmanager->feedback_op[0]=dyn_manager_bulk_op_new(mmanager->dyn_module.manager,DYN_MANAGER_READ,num,ids,address,length,(unsigned char * const*)data);
+ if(num>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)
@@ -258,10 +293,10 @@ unsigned char mmanager_init(TMotionManager *mmanager,TMemory *memory,unsigned sh
mmanager->mem_module.read_cmd=mmanager_read_cmd;
if(!mem_module_add_ram_segment(&mmanager->mem_module,ram_base_address+RAM_DYN_MODULE_LENGTH,RAM_MM_LENGTH))
return 0x00;
- mmanager->ram_base_address=ram_base_address;
+ mmanager->ram_base_address=ram_base_address+RAM_DYN_MODULE_LENGTH;
if(!mem_module_add_eeprom_segment(&mmanager->mem_module,eeprom_base_address+EEPROM_DYN_MODULE_LENGTH,EEPROM_MM_LENGTH))
return 0x00;
- mmanager->eeprom_base_address=eeprom_base_address;
+ mmanager->eeprom_base_address=eeprom_base_address+EEPROM_DYN_MODULE_LENGTH;
if(!mem_add_module(memory,&mmanager->mem_module))
return 0x00;
mmanager->memory=memory;