Fixed some problems of inicialization and added a pan_tilt exception class....

Fixed some problems of inicialization and added a pan_tilt exception class. Improved the error handling

src/CMakeLists.txt

 ADD_SUBDIRECTORY(xml)
 
 # edit the following line to add all the source code files of the library
-SET(sources dynamixel_pan_tilt.cpp)
+SET(sources dynamixel_pan_tilt.cpp dynamixel_pan_tilt_exceptions.cpp)
 
 # edit the following line to add all the header files of the library
-SET(headers dynamixel_pan_tilt.h)
+SET(headers dynamixel_pan_tilt.h dynamixel_pan_tilt_exceptions.h)
 
 FIND_PACKAGE(iriutils REQUIRED)
 

src/dynamixel_pan_tilt.h

-// Copyright (C) 2016 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
+// Copyright (C) 2016-2017 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
 // Author Alejandro Lopez Gestoso (alopez@iri.upc.edu)
 // All rights reserved.
 //
@@ -23,6 +23,7 @@
 #include "dynamixelserver.h"
 #include "dynamixelexceptions.h"
 #include "dynamixel_motor_exceptions.h"
+#include "dynamixel_pan_tilt_exceptions.h"
 #include <stdio.h>
 #include <stdlib.h>
 #include <string.h>
@@ -201,12 +202,13 @@ typedef struct
  * 
  * \b - alarm_shtdwn: 5 (Input Voltage error and Overheating error)
  * \b - temp_limit: 85
- * \b - max_torque: 100
+ * \b - max_torque: 50
  * \b - check_hyst_offset: 2.404867
  * \b - check_hyst_slope: 0.20688
  *
  * You can also configurate the following parameters specifically for each servo (if not, both servos get the same configuration value):
  *
+ * \b - max_torque
  * \b - cw_comp_margin
  * \b - ccw_comp_margin
  * \b - cw_comp_slope
@@ -245,8 +247,8 @@ class CDynamixel_Pan_Tilt
     Dynamixel_pan_tilt_data checking_hysteresis, checking_hysteresis_offset, checking_hysteresis_slope;
 
     CEventServer *event_server;
-    const std::string exiting;
-    const std::string torque_moving;
+    std::string exiting;
+    std::string torque_moving;
 
 
   protected:
@@ -604,6 +606,20 @@ class CDynamixel_Pan_Tilt
      */ 
     void set_pid_control(Dynamixel_pan_tilt_pid &pid);
 
+    /**
+     * \brief Function to set the max torque of both servos. 
+     *  
+     * \param pid The torque value.
+     */ 
+    void set_max_torque(Dynamixel_pan_tilt_data &torque);
+
+    /**
+     * \brief Function to get the current limit torque of both servos in %.
+     * 
+     * \return A struct with the current limit torque of both servos in %.
+     */
+    Dynamixel_pan_tilt_data get_limit_torque(void);
+
     /**
      * \brief User firendly function to print the absolute position of the pan-tilt.
      *

src/dynamixel_pan_tilt_exceptions.cpp

+// Copyright (C) 2016-2017 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
+// Author Alejandro Lopez Gestoso (alopez@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of iriutils
+// iriutils is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program.  If not, see <http://www.gnu.org/licenses/>.
+
+#include "dynamixel_pan_tilt_exceptions.h"
+#include <string.h>
+#include <stdio.h>
+
+const std::string dynpantilt_exception_msg="[CDynamixel_Pan_Tilt class] - ";
+
+CDynamixel_Pan_TiltException::CDynamixel_Pan_TiltException(const std::string& where,const std::string& error_msg):CException(where,dynpantilt_exception_msg)
+{
+  this->error_msg+=error_msg;
+}
\ No newline at end of file

src/dynamixel_pan_tilt_exceptions.h

+// Copyright (C) 2016-2017 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
+// Author Alejandro Lopez Gestoso (alopez@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of iriutils
+// iriutils is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program.  If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef DYNPANTILT_EXCEPTIONS
+#define DYNPANTILT_EXCEPTIONS
+
+#include "exceptions.h"
+#include <string>
+
+/**
+ * \brief Dynamixel_pan_tilt exception class
+ *
+ * This class implements the exceptions for the CDynamixel_Pan_Tilt class. 
+ * 
+ * Similarly to other exception classes, it appends a class identifer
+ * string ("[CDynamixel_Pan_Tilt class] - ") to the error message in order to identify the
+ * class that generated the exception.
+ * 
+ * The base class can be used to catch any exception thrown by the application
+ * or also, this class can be used in order to catch only exceptions generated 
+ * by CDynamixel_Pan_Tilt objects.
+ *
+ */
+class CDynamixel_Pan_TiltException : public CException
+{
+  public:
+    /**
+     * \brief Constructor
+     *
+     * The constructor calls the base class constructor to add the general
+     * exception identifier and then adds the class identifier string 
+     * "[CDynamixel_Pan_Tilt class]" and the supplied error message. 
+     * 
+     * 
+     * \verbatim
+     * [Exception caught] - <where>
+     * [CDynamixel_Pan_Tilt class] - <error message> 
+     * \endverbatim
+     * 
+     * \param where a null terminated string with the information about the name
+     *              of the function, the source code filename and the line where
+     *              the exception was generated. This string must be generated 
+     *              by the _HERE_ macro.
+     *
+     * \param error_msg a null terminated string that contains the error message.
+     *                  This string may have any valid character and there is no 
+     *                  limit on its length.
+     *
+     *                 
+     */
+    CDynamixel_Pan_TiltException(const std::string& where,const std::string& error_msg);
+};
+
+#endif

src/examples/CMakeLists.txt

@@ -2,3 +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)

src/examples/test_dynamixel_pan_tilt.cpp

-// Copyright (C) 2016 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
+// Copyright (C) 2016-2017 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
 // Author Alejandro Lopez Gestoso (alopez@iri.upc.edu)
 // All rights reserved.
 //
@@ -122,7 +122,7 @@
  * \endverbatim
  */
 
-std::string name="PAN-TILT-AX-12+";
+std::string name="PAN_TILT_AX_12plus";
 std::string config_file="../src/xml/dyn_pan_tilt_config_AX12plus.xml";
 std::string config_saving_file = "../src/xml/saving_config.xml";
 
@@ -212,7 +212,7 @@ int main(int argc, char *argv[])
       //*/
 
       pan_tilt->get_compliance_control(compliance);
-      double max_angle_error =   fabs(0.5*compliance.pan.cw_compliance_margin); //max angle error permitted
+      double max_angle_error =   fabs(0.7*compliance.pan.cw_compliance_margin); //max angle error permitted
       //double max_angle_error = 0.7;
       double time_interval   =   0.1; //time in secs between checks
       double max_time_sec    =   5.0; //max time to wait until timeout
@@ -245,7 +245,7 @@ int main(int argc, char *argv[])
       while((fabs(pos.pan - desired_pos.pan) > max_angle_error || fabs(pos.tilt - desired_pos.tilt) > max_angle_error) && t<timeout)
       {
         pos = pan_tilt->get_position();
-        pan_tilt->print_current_position();
+        //pan_tilt->print_current_position();
         usleep(uperiod);
         t++;
       }
@@ -345,7 +345,7 @@ int main(int argc, char *argv[])
       while((fabs(pos.pan - desired_pos.pan) > max_angle_error || fabs(pos.tilt - desired_pos.tilt) > max_angle_error) && t<timeout)
       {
         pos = pan_tilt->get_position();
-        pan_tilt->print_current_position();
+        //pan_tilt->print_current_position();
         usleep(uperiod);
         t++;
       }
@@ -428,6 +428,8 @@ int main(int argc, char *argv[])
   catch(CException &e)
   {
     std::cout << "[Genereal exception]: " << e.what() << std::endl;
+    if (pan_tilt != NULL)
+      pan_tilt->stop();
   }
 
   if(pan_tilt!=NULL)

src/xml/dynamixel_pan_tilt_cfg_file.hxx

@@ -394,20 +394,19 @@ class dynamixel_pan_tilt_config_t: public ::xml_schema::type
   // max_torque
   // 
   typedef ::xml_schema::float_ max_torque_type;
-  typedef ::xsd::cxx::tree::optional< max_torque_type > max_torque_optional;
+  typedef ::xsd::cxx::tree::sequence< max_torque_type > max_torque_sequence;
+  typedef max_torque_sequence::iterator max_torque_iterator;
+  typedef max_torque_sequence::const_iterator max_torque_const_iterator;
   typedef ::xsd::cxx::tree::traits< max_torque_type, char > max_torque_traits;
 
-  const max_torque_optional&
+  const max_torque_sequence&
   max_torque () const;
 
-  max_torque_optional&
+  max_torque_sequence&
   max_torque ();
 
   void
-  max_torque (const max_torque_type& x);
-
-  void
-  max_torque (const max_torque_optional& x);
+  max_torque (const max_torque_sequence& s);
 
   // cw_comp_margin
   // 
@@ -645,7 +644,7 @@ class dynamixel_pan_tilt_config_t: public ::xml_schema::type
   temp_limit_optional temp_limit_;
   ::xsd::cxx::tree::one< max_voltage_type > max_voltage_;
   ::xsd::cxx::tree::one< min_voltage_type > min_voltage_;
-  max_torque_optional max_torque_;
+  max_torque_sequence max_torque_;
   cw_comp_margin_sequence cw_comp_margin_;
   ccw_comp_margin_sequence ccw_comp_margin_;
   cw_comp_slope_sequence cw_comp_slope_;

src/xml/dynamixel_pan_tilt_cfg_file.xsd

@@ -32,7 +32,7 @@ copyright : not copyrighted - public domain
       </xsd:element>
       <xsd:element name="min_voltage" type="xsd:float">
       </xsd:element>
-      <xsd:element name="max_torque" type="xsd:float" minOccurs="0">
+      <xsd:element name="max_torque" type="xsd:float" minOccurs="0" maxOccurs="2">
       </xsd:element>
       <xsd:element name="cw_comp_margin" type="xsd:unsignedByte" maxOccurs="2">
       </xsd:element>