diff --git a/src/dynamixel_motor.cpp b/src/dynamixel_motor.cpp
index 1db90d287811f207704fdb93023c85d79fa9fa7e..a03027e4a29bb0f17445070954bf7f72034aa898 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 ccf412aabfb9aaadf8957d6b59cb681ec10fd2f8..e285931378591fae57494b533d3b08261c1b301c 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 0000000000000000000000000000000000000000..60fab00e51a23b8c7521a8221dabb2318eba2065
--- /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 0000000000000000000000000000000000000000..0ee478e56e7097f0c5f7e74e5e1c044fc8577cfb
--- /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 4443ce90dd3a62891ef5d251f8ee590b2ab1e99b..6493d42f971570e948cbfa0a1fe6c4ed3259dcdc 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 ce3a3a89b5c5f623f1ce9cdf6896ff74d3631a99..a4b6d7f7d005b63a3701349fa4aeaa1b611151f2 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 0843898674d206ab2b5e622d22d67df39777d05f..8cb70e531d4d274cd444c46073f8f9cafec925d2 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})