From 7ff7e51b0676543b7f9ec21a31516744343702e7 Mon Sep 17 00:00:00 2001
From: Sergi Hernandez Juan <shernand@iri.upc.edu>
Date: Tue, 7 Apr 2020 20:31:43 +0200
Subject: [PATCH] Added the functions to power the robot on and off. Modified
the manger example.
---
include/darwin_dyn_manager.h | 3 +++
include/darwin_mmanager.h | 3 ---
src/darwin_dyn_manager.cpp | 21 +++++++++++++++++++++
src/examples/darwin_manager_test.cpp | 4 ++++
4 files changed, 28 insertions(+), 3 deletions(-)
diff --git a/include/darwin_dyn_manager.h b/include/darwin_dyn_manager.h
index f66ceed..f8a9336 100644
--- a/include/darwin_dyn_manager.h
+++ b/include/darwin_dyn_manager.h
@@ -20,6 +20,9 @@ class CDarwinDynManager : public CDarwinRobotBase
bool is_running(void);
void start_scan(void);
bool is_scanning(void);
+ void enable_power(void);
+ void disable_power(void);
+ bool is_power_enabled(void);
unsigned char get_num_devices(void);
unsigned int get_present_devices(void);
~CDarwinDynManager();
diff --git a/include/darwin_mmanager.h b/include/darwin_mmanager.h
index 1166724..e5080a0 100644
--- a/include/darwin_mmanager.h
+++ b/include/darwin_mmanager.h
@@ -25,9 +25,6 @@ class CDarwinMManager : public CDarwinDynManager
void assign_module(unsigned char servo_id, mm_mode_t mode);
void assign_module(std::string &servo,std::string &module);
mm_mode_t get_module(unsigned char servo_id);
-// void enable_power(void);
-// void disable_power(void);
-// bool is_power_enabled(void);
std::vector<double> get_servo_angles(void);
double get_servo_angle(unsigned char servo_id);
void set_servo_offset(unsigned char servo_id,double offset);
diff --git a/src/darwin_dyn_manager.cpp b/src/darwin_dyn_manager.cpp
index f81fd93..cc5e6cd 100644
--- a/src/darwin_dyn_manager.cpp
+++ b/src/darwin_dyn_manager.cpp
@@ -79,6 +79,27 @@ bool CDarwinDynManager::is_scanning(void)
return scanning&MANAGER_SCANNING;
}
+void CDarwinDynManager::enable_power(void)
+{
+ this->is_valid();
+ this->robot_device->write_byte_register(MANAGER_CONTROL,MANAGER_ENABLE_POWER);
+}
+
+void CDarwinDynManager::disable_power(void)
+{
+ this->is_valid();
+ this->robot_device->write_byte_register(MANAGER_CONTROL,MANAGER_DISABLE_POWER);
+}
+
+bool CDarwinDynManager::is_power_enabled(void)
+{
+ unsigned char powered;
+
+ this->is_valid();
+ this->robot_device->read_byte_register(MANAGER_CONTROL,&powered);
+ return powered&MANAGER_POWERED;
+}
+
unsigned char CDarwinDynManager::get_num_devices(void)
{
unsigned char num;
diff --git a/src/examples/darwin_manager_test.cpp b/src/examples/darwin_manager_test.cpp
index 26a5fd1..fe09ca6 100644
--- a/src/examples/darwin_manager_test.cpp
+++ b/src/examples/darwin_manager_test.cpp
@@ -21,6 +21,9 @@ int main(int argc, char *argv[])
std::cout << "Number of modules: " << darwin.get_num_modules() << std::endl;
std::cout << "Number of masters: " << darwin.get_num_masters() << std::endl;
std::cout << "Motion manager period: " << darwin.get_period() << std::endl;
+
+ darwin.enable_power();
+ sleep(2);
darwin.stop();
darwin.start_scan();
while(darwin.is_scanning())
@@ -66,6 +69,7 @@ int main(int argc, char *argv[])
usleep(100000);
}
darwin.stop();
+ darwin.disable_power();
}catch(CException &e){
std::cout << e.what() << std::endl;
}
--
GitLab