From 7f6e003057ef68847e59c691020f740027a6414f Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Sergi=20Hern=C3=A0ndez=20Juan?= <shernand@iri.upc.edu>
Date: Mon, 6 Feb 2012 15:34:21 +0000
Subject: [PATCH] Added an exception class

---
 src/dynamixel_motor.cpp            |  5 +++
 src/dynamixel_motor.h              |  5 +++
 src/dynamixel_motor_exceptions.cpp | 22 ++++++++++++
 src/dynamixel_motor_exceptions.h   | 36 ++++++++++++++++++++
 src/dynamixel_motor_group.cpp      | 54 +++++++++++++++++++++++++-----
 src/dynamixel_motor_group.h        | 29 +++++++---------
 src/examples/CMakeLists.txt        |  6 ++++
 7 files changed, 132 insertions(+), 25 deletions(-)
 create mode 100755 src/dynamixel_motor_exceptions.cpp
 create mode 100755 src/dynamixel_motor_exceptions.h

diff --git a/src/dynamixel_motor.cpp b/src/dynamixel_motor.cpp
index 1db90d2..a03027e 100644
--- a/src/dynamixel_motor.cpp
+++ b/src/dynamixel_motor.cpp
@@ -643,6 +643,11 @@ std::string CDynamixelMotor::get_model(void)
   return this->info.model;
 }
 
+int CDynamixelMotor::get_id(void)
+{
+  return this->dynamixel_dev->get_id();
+}
+
 unsigned char CDynamixelMotor::get_firmware_version(void)
 {
 
diff --git a/src/dynamixel_motor.h b/src/dynamixel_motor.h
index ccf412a..e285931 100644
--- a/src/dynamixel_motor.h
+++ b/src/dynamixel_motor.h
@@ -264,6 +264,11 @@ class CDynamixelMotor : public CMotorControl
      *  
      */ 
     std::string get_model(void);
+    /**
+     * \brief 
+     *  
+     */ 
+    int get_id(void);
     /**
      * \brief 
      *  
diff --git a/src/dynamixel_motor_exceptions.cpp b/src/dynamixel_motor_exceptions.cpp
new file mode 100755
index 0000000..60fab00
--- /dev/null
+++ b/src/dynamixel_motor_exceptions.cpp
@@ -0,0 +1,22 @@
+#include "dynamixel_motor_exceptions.h"
+#include <sstream>
+#include <string.h>
+#include <stdio.h>
+
+const std::string dynamixel_motor_error_message="DynamixelMotor error - ";
+const std::string dynamixel_motor_group_error_message="DynamixelMotorGroup error - ";
+
+CDynamixelMotorException::CDynamixelMotorException(const std::string& where,const std::string &error_msg):CException(where,dynamixel_motor_error_message)
+{
+  std::stringstream text;
+ 
+  this->error_msg+=error_msg;
+}
+
+CDynamixelMotorGroupException::CDynamixelMotorGroupException(const std::string& where,const std::string &error_msg):CException(where,dynamixel_motor_group_error_message)
+{
+  std::stringstream text;
+ 
+  this->error_msg+=error_msg;
+}
+
diff --git a/src/dynamixel_motor_exceptions.h b/src/dynamixel_motor_exceptions.h
new file mode 100755
index 0000000..0ee478e
--- /dev/null
+++ b/src/dynamixel_motor_exceptions.h
@@ -0,0 +1,36 @@
+#ifndef _DYNAMIXEL_MOTOR_EXCEPTIONS
+#define _DYNAMIXEL_MOTOR_EXCEPTIONS
+
+#include "exceptions.h"
+
+/**
+ * \brief Dynamixel exception
+ * 
+ * This class is used to report general errors of the dynamixel devices. These 
+ * errors are normally caused by invalid parameters passed to the functions.
+ * In this case the error message is used to identify which error has ocuurred.
+ *
+ */
+class CDynamixelMotorException : public CException
+{
+  public:
+    /**
+     * \brief  
+     */
+    CDynamixelMotorException(const std::string& where,const std::string &error_msg);
+};
+
+/**
+ * \brief 
+ */
+class CDynamixelMotorGroupException : public CException
+{
+  public:
+    /**
+     * \brief
+     *
+     */
+    CDynamixelMotorGroupException(const std::string& where,const std::string &error_msg);
+};
+
+#endif
diff --git a/src/dynamixel_motor_group.cpp b/src/dynamixel_motor_group.cpp
index 4443ce9..6493d42 100644
--- a/src/dynamixel_motor_group.cpp
+++ b/src/dynamixel_motor_group.cpp
@@ -17,12 +17,11 @@ CDynamixelMotorGroup::CDynamixelMotorGroup(std::string &group_id):CMotorGroup(gr
   else 
   {
     this->dyn_server=CDynamixelServer::instance();
-    this->servo_id.resize(4);
-    this->servo_id[0]=0x02;
-    this->servo_id[1]=0x04;
-    this->servo_id[2]=0x03;
-    this->servo_id[3]=0x05;
-
+//    this->servo_id.resize(4);
+//    this->servo_id[0]=0x02;
+//    this->servo_id[1]=0x04;
+//    this->servo_id[2]=0x03;
+//    this->servo_id[3]=0x05;
   }
 }
 
@@ -65,8 +64,8 @@ void CDynamixelMotorGroup::move(std::vector<float> &position,std::vector<float>
 
 	data[i][0]=((int)position[i])%256;
 	data[i][1]=position[i]/256;
-	data[i][2]=0xFF;
-	data[i][3]=0x03;
+	data[i][2]=0x00;
+	data[i][3]=0x00;
       }
       dyn_server->write_sync(servo_id,star_addrs,data);
     }catch(CException &e){
@@ -76,6 +75,45 @@ void CDynamixelMotorGroup::move(std::vector<float> &position,std::vector<float>
   }  
 }
 
+std::string CDynamixelMotorGroup::add_motor_control(CDynamixelMotor *controller)
+{
+  std::string cont_id;
+
+  try{
+    cont_id=CMotorGroup::add_motor_control(controller);
+    // if the controller is successfully added
+    this->servo_id.push_back(((CDynamixelMotor *)controller)->get_id());
+  }catch(CException &e){
+    /* handle exceptions */
+    throw;
+  }
+
+  return cont_id;
+}
+
+void CDynamixelMotorGroup::remove_motor_control(std::string &cont_id)
+{
+  std::vector<TMotorInfo>::iterator old_cont;
+  std::vector<unsigned char> new_servo_id;
+  unsigned int i,old_id;
+
+  try{
+    if((old_cont=this->search_controller(cont_id))==(std::vector<TMotorInfo>::iterator)NULL)
+    {
+      /* handle exceptions */
+      throw CMotorGroupException(_HERE_,"Impossible to remove the CMotorControl object, no object exist with the given identifier");
+    }
+    old_id=((CDynamixelMotor *)old_cont->controller)->get_id();
+    for(i=0;i<this->servo_id.size();i++)
+      if(this->servo_id[i]!=old_id)
+        new_servo_id.push_back(servo_id[i]);
+    this->servo_id=new_servo_id;
+    CMotorGroup::remove_motor_control(cont_id);
+  }catch(CException &e){
+    /* handle exceptions */
+    throw;
+  }
+}
 
 CDynamixelMotorGroup::~CDynamixelMotorGroup()
 {
diff --git a/src/dynamixel_motor_group.h b/src/dynamixel_motor_group.h
index ce3a3a8..a4b6d7f 100644
--- a/src/dynamixel_motor_group.h
+++ b/src/dynamixel_motor_group.h
@@ -1,31 +1,26 @@
-#ifndef _DYNAMIXEL_MOTOR_GROUP
-#define _DYNAMIXEL_MOTOR_GROUP
+#ifndef _DYNAMIXEL_MOTOR_GROUP_H
+#define _DYNAMIXEL_MOTOR_GROUP_H
 
+#include "dynamixel_motor.h"
 #include "threadserver.h"
 #include "eventserver.h"
 #include "motor_group.h"
 #include "mutex.h"
 #include <vector>
 
-
 class CDynamixelMotorGroup  : public CMotorGroup
 {
 
-		private:
-			
-			std::vector<unsigned char> servo_id;
-			
-			CDynamixelServer *dyn_server;
-			
-		public: 
-		
-			CDynamixelMotorGroup(std::string &group_id);
-			
-			
-		   void move(std::vector<float> &position,std::vector<float> &velocity,std::vector<float> &acceleration);	
+  private:
+    std::vector<unsigned char> servo_id;
+    CDynamixelServer *dyn_server;
 
-
-			virtual ~CDynamixelMotorGroup();
+  public: 
+    CDynamixelMotorGroup(std::string &group_id);
+    void move(std::vector<float> &position,std::vector<float> &velocity,std::vector<float> &acceleration);	
+    std::string add_motor_control(CDynamixelMotor *controller);
+    void remove_motor_control(std::string &cont_id);    
+    virtual ~CDynamixelMotorGroup();
 };
 
 
diff --git a/src/examples/CMakeLists.txt b/src/examples/CMakeLists.txt
index 0843898..8cb70e5 100644
--- a/src/examples/CMakeLists.txt
+++ b/src/examples/CMakeLists.txt
@@ -27,3 +27,9 @@ ADD_EXECUTABLE(test_dynamixel_motor_open_loop test_dynamixel_motor_open_loop.cpp
 
 # edit the following line to add the necessary libraries
 TARGET_LINK_LIBRARIES(test_dynamixel_motor_open_loop dynamixel_motor_cont ${comm_LIBRARY} ${motor_control_LIBRARY})
+
+# edit the following line to add the source code for the example and the name of the executable
+ADD_EXECUTABLE(test_dynamixel_motor_group_open_loop test_dynamixel_motor_group_open_loop.cpp)
+
+# edit the following line to add the necessary libraries
+TARGET_LINK_LIBRARIES(test_dynamixel_motor_group_open_loop dynamixel_motor_cont ${comm_LIBRARY} ${motor_control_LIBRARY})
-- 
GitLab