diff --git a/dynamixel_manager/include/dyn_manager.h b/dynamixel_manager/include/dyn_manager.h index e82ffc5d997d5613a44af1666b5e4bc31c2f4f7b..1f1ce893cfa0908b9d26a555986f9a12b2fe0dd2 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 c7e37603271f9a28d6f40e92dea1048af36cecb6..37ae88698ac8091061db4046cef3120a655e015e 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 b9282ff446e165f93de35bf79334bbae8d3aa43a..99d42c5ce8439eba246f927e970b885b2a969f64 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;