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