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