From 3f12b46831208b4164879da9f2aef374c5bbbf6b Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Alejandro=20L=C3=B3pez=20Gestoso?= <alopez@iri.upc.edu>
Date: Fri, 20 May 2016 09:15:18 +0000
Subject: [PATCH] Added a static function to get the configuration values of
the pan&tilt from an xml file.
---
src/dynamixel_pan_tilt.cpp | 49 ++++++++++++++++++++++++
src/dynamixel_pan_tilt.h | 27 +++++++++++++
src/examples/CMakeLists.txt | 5 ++-
src/examples/test_dynamixel_pan_tilt.cpp | 7 ++++
src/xml/dyn_pan_tilt_config_AX12plus.xml | 10 ++---
5 files changed, 92 insertions(+), 6 deletions(-)
diff --git a/src/dynamixel_pan_tilt.cpp b/src/dynamixel_pan_tilt.cpp
index 3d18207..f1001b6 100644
--- a/src/dynamixel_pan_tilt.cpp
+++ b/src/dynamixel_pan_tilt.cpp
@@ -358,6 +358,55 @@ void CDynamixel_Pan_Tilt::load_config(std::string &filename)
throw CDynamixel_Pan_TiltException(_HERE_,"The dynamixel pan&tilt is not properly configured. Configuration not loaded");
}
+void CDynamixel_Pan_Tilt::read_config(std::string &filename, Dynamixel_pan_tilt_xml_limits &xml_limits)
+{
+ struct stat buffer;
+
+ if(stat(filename.c_str(),&buffer)==0)
+ {
+ // try to open the specified file
+ try{
+ std::auto_ptr<dynamixel_pan_tilt_config_t> cfg(dynamixel_pan_tilt_config(filename.c_str(), xml_schema::flags::dont_validate));
+ // configure the parameters of the controller
+
+ dynamixel_pan_tilt_config_t::min_angle_const_iterator min_angle (cfg->min_angle().begin());
+ dynamixel_pan_tilt_config_t::max_angle_const_iterator max_angle (cfg->max_angle().begin());
+ xml_limits.min_angle.pan = *min_angle;
+ xml_limits.max_angle.pan = *max_angle;
+
+ min_angle = cfg->min_angle().end();
+ max_angle = cfg->max_angle().end();
+ min_angle--;
+ max_angle--;
+ xml_limits.min_angle.tilt = *min_angle;
+ xml_limits.max_angle.tilt = *max_angle;
+
+ if (cfg->max_torque().size() > 0)
+ {
+ dynamixel_pan_tilt_config_t::max_torque_const_iterator max_torque (cfg->max_torque().begin());
+ xml_limits.max_torque.pan = *max_torque;
+ max_torque = cfg->max_torque().end();
+ max_torque--;
+ xml_limits.max_torque.tilt = *max_torque;
+ }
+ else
+ {
+ xml_limits.max_torque.pan = 50;
+ xml_limits.max_torque.tilt = 50;
+ }
+ }
+ catch (const xml_schema::exception& e)
+ {
+ std::ostringstream os;
+ os << "loading config exception: " << e;
+ /* handle exceptions */
+ throw CDynamixelMotorException(_HERE_,os.str());
+ }
+ }
+ else
+ throw CDynamixel_Pan_TiltException(_HERE_,"The configuration file does not exist.");
+}
+
void CDynamixel_Pan_Tilt::save_config(std::string &filename)
{
/*xml_schema::namespace_infomap map;
diff --git a/src/dynamixel_pan_tilt.h b/src/dynamixel_pan_tilt.h
index 9b4c3a7..52871dd 100644
--- a/src/dynamixel_pan_tilt.h
+++ b/src/dynamixel_pan_tilt.h
@@ -188,6 +188,25 @@ typedef struct
Torque_moving_state tilt;
}Torque_pan_tilt_moving_state;
+/**
+ * \struct Dynamixel_pan_tilt_xml_limits.
+ *
+ * \brief A struct to save the configuration limits readed of a xml file.
+ *
+ * It has three members of type Dynamixel_pan_tilt_data:
+ * - max_torque
+ * - max_angle
+ * - min_angle
+ */
+
+typedef struct
+{
+ Dynamixel_pan_tilt_data max_torque;
+ Dynamixel_pan_tilt_data max_angle;
+ Dynamixel_pan_tilt_data min_angle;
+}Dynamixel_pan_tilt_xml_limits;
+
+
/**
* \class CDynamyxel_Pan_Tilt
*
@@ -443,6 +462,14 @@ class CDynamixel_Pan_Tilt
* \param filename The file where it's going to be read the pan configuration.
*/
void load_config(std::string &filename);
+
+ /**
+ * \brief Static function to read the configuration of the pan-tilt reading the information from a specific file.
+ *
+ * \param filename The file where it's going to be read the pan configuration.
+ * \param xml_limits The struct where is going to be saved the information.
+ */
+ static void read_config(std::string &filename, Dynamixel_pan_tilt_xml_limits &xml_limits);
/**
* \brief Function to save the current configuration of the pan-tilt on a specific file.
diff --git a/src/examples/CMakeLists.txt b/src/examples/CMakeLists.txt
index 70de73b..fd57ded 100644
--- a/src/examples/CMakeLists.txt
+++ b/src/examples/CMakeLists.txt
@@ -2,4 +2,7 @@
ADD_EXECUTABLE(test_dynamixel_pan_tilt test_dynamixel_pan_tilt.cpp)
# link necessary libraries
TARGET_LINK_LIBRARIES(test_dynamixel_pan_tilt dynamixel_pan_tilt)
-
+# create an example application
+ADD_EXECUTABLE(fucking_test fucking_test.cpp)
+# link necessary libraries
+TARGET_LINK_LIBRARIES(fucking_test dynamixel_pan_tilt)
diff --git a/src/examples/test_dynamixel_pan_tilt.cpp b/src/examples/test_dynamixel_pan_tilt.cpp
index 30ed6dc..de44d89 100644
--- a/src/examples/test_dynamixel_pan_tilt.cpp
+++ b/src/examples/test_dynamixel_pan_tilt.cpp
@@ -209,6 +209,13 @@ int main(int argc, char *argv[])
sleep(1);
//pan_tilt->save_config(config_saving_file);
+
+ Dynamixel_pan_tilt_xml_limits xml_limits;
+ CDynamixel_Pan_Tilt::read_config(config_file, xml_limits);
+ pan_tilt->print_pan_tilt_data(xml_limits.max_angle);
+ pan_tilt->print_pan_tilt_data(xml_limits.min_angle);
+ pan_tilt->print_pan_tilt_data(xml_limits.max_torque);
+
//*/
pan_tilt->get_compliance_control(compliance);
diff --git a/src/xml/dyn_pan_tilt_config_AX12plus.xml b/src/xml/dyn_pan_tilt_config_AX12plus.xml
index abc5082..ba3b597 100644
--- a/src/xml/dyn_pan_tilt_config_AX12plus.xml
+++ b/src/xml/dyn_pan_tilt_config_AX12plus.xml
@@ -7,7 +7,7 @@
<temp_limit>85</temp_limit>
<max_voltage>19</max_voltage>
<min_voltage>6</min_voltage>
- <max_torque>20</max_torque>
+ <max_torque>60</max_torque>
<cw_comp_margin>2</cw_comp_margin>
<ccw_comp_margin>2</ccw_comp_margin>
<cw_comp_slope>64</cw_comp_slope>
@@ -16,8 +16,8 @@
<kp>0</kp>
<ki>0</ki>
<kd>0</kd>
- <min_angle>-95</min_angle><!--PAN -->
- <max_angle>95</max_angle><!--PAN -->
- <min_angle>-95</min_angle><!--TILT -->
- <max_angle>60</max_angle><!--TILT -->
+ <min_angle>-85</min_angle><!--PAN -->
+ <max_angle>85</max_angle><!--PAN -->
+ <min_angle>-85</min_angle><!--TILT -->
+ <max_angle>85</max_angle><!--TILT -->
</dynamixel_pan_tilt_config>
--
GitLab