From f7b4c0e564d80dbeb5ef8c3019e6269dde58018f Mon Sep 17 00:00:00 2001 From: Sergi Hernandez Juan <shernand@iri.upc.edu> Date: Thu, 2 Apr 2020 22:45:52 +0200 Subject: [PATCH] Added the feature to turn the dynamixel masters power on and off. Added two bits to the control register of the dynamixel manager. --- dynamixel_manager/include/dyn_manager.h | 2 ++ .../include/dyn_manager_registers.h | 7 ++++-- dynamixel_manager/src/dyn_manager.c | 22 +++++++++++++++++++ 3 files changed, 29 insertions(+), 2 deletions(-) diff --git a/dynamixel_manager/include/dyn_manager.h b/dynamixel_manager/include/dyn_manager.h index e82ffc5..1f1ce89 100644 --- a/dynamixel_manager/include/dyn_manager.h +++ b/dynamixel_manager/include/dyn_manager.h @@ -147,6 +147,8 @@ static inline void dyn_manager_wait_stopped(TDynManager *manager) } void dyn_manager_stop(TDynManager *manager); void dyn_manager_scan(TDynManager *manager); +void dyn_manager_enable_power(TDynManager *manager); +void dyn_manager_disable_power(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 c7e3760..37ae886 100644 --- a/dynamixel_manager/include/dyn_manager_registers.h +++ b/dynamixel_manager/include/dyn_manager_registers.h @@ -5,11 +5,14 @@ #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_CONTROL_OFFSET 2 // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0 + // scanning | running | powered | disable power | enable power | start scan | stop | start #define DYN_MANAGER_START 0x01 #define DYN_MANAGER_STOP 0x02 #define DYN_MANAGER_START_SCAN 0x04 +#define DYN_MANAGER_ENABLE_POWER 0x08 +#define DYN_MANAGER_DISABLE_POWER 0x10 +#define DYN_MANAGER_POWERED 0x20 #define DYN_MANAGER_RUNNING 0x40 #define DYN_MANAGER_SCANNING 0x80 diff --git a/dynamixel_manager/src/dyn_manager.c b/dynamixel_manager/src/dyn_manager.c index b9282ff..99d42c5 100644 --- a/dynamixel_manager/src/dyn_manager.c +++ b/dynamixel_manager/src/dyn_manager.c @@ -53,6 +53,10 @@ void dyn_manager_write_cmd(TDynManager *module,unsigned short int address,unsign dyn_manager_stop(module); if(data[DYN_MANAGER_CONTROL_OFFSET-ram_offset]&DYN_MANAGER_START_SCAN) dyn_manager_start_scan(module); + if(data[DYN_MANAGER_CONTROL_OFFSET-ram_offset]&DYN_MANAGER_ENABLE_POWER) + dyn_manager_enable_power(module); + if(data[DYN_MANAGER_CONTROL_OFFSET-ram_offset]&DYN_MANAGER_DISABLE_POWER) + dyn_manager_disable_power(module); } } @@ -411,6 +415,24 @@ void dyn_manager_scan(TDynManager *manager) manager->memory->data[manager->ram_base_address+DYN_MANAGER_CONTROL_OFFSET]&=(~DYN_MANAGER_SCANNING); } +void dyn_manager_enable_power(TDynManager *manager) +{ + unsigned char i; + + for(i=0;i<manager->num_masters;i++) + manager->masters[i]->enable_power(); + manager->memory->data[manager->ram_base_address+DYN_MANAGER_CONTROL_OFFSET]|=DYN_MANAGER_POWERED; +} + +void dyn_manager_disable_power(TDynManager *manager) +{ + unsigned char i; + + for(i=0;i<manager->num_masters;i++) + manager->masters[i]->disable_power(); + manager->memory->data[manager->ram_base_address+DYN_MANAGER_CONTROL_OFFSET]&=(~DYN_MANAGER_POWERED); +} + void dyn_manager_add_master(TDynManager *manager,TDynamixelMaster *master) { unsigned char i; -- GitLab