Added an exception class

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)
 {
 

src/dynamixel_motor.h

@@ -264,6 +264,11 @@ class CDynamixelMotor : public CMotorControl
      *  
      */ 
     std::string get_model(void);
+    /**
+     * \brief 
+     *  
+     */ 
+    int get_id(void);
     /**
      * \brief 
      *  

src/dynamixel_motor_exceptions.cpp

+#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;
+}
+

src/dynamixel_motor_exceptions.h

+#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

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()
 {

src/dynamixel_motor_group.h

-#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();
 };
 
 

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})