From 553c7f75d88937fecaa580e6b9d5ca7b5ae89a37 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Alejandro=20L=C3=B3pez=20Gestoso?= <alopez@iri.upc.edu>
Date: Mon, 2 May 2016 08:16:04 +0000
Subject: [PATCH] Fixed some problems of inicialization and added a pan_tilt
exception class. Improved the error handling
---
src/CMakeLists.txt | 4 +-
src/dynamixel_pan_tilt.cpp | 1080 ++++++++++++++--------
src/dynamixel_pan_tilt.h | 24 +-
src/dynamixel_pan_tilt_exceptions.cpp | 28 +
src/dynamixel_pan_tilt_exceptions.h | 69 ++
src/examples/CMakeLists.txt | 4 +
src/examples/test_dynamixel_pan_tilt.cpp | 12 +-
src/xml/dynamixel_pan_tilt_cfg_file.hxx | 15 +-
src/xml/dynamixel_pan_tilt_cfg_file.xsd | 2 +-
9 files changed, 831 insertions(+), 407 deletions(-)
create mode 100644 src/dynamixel_pan_tilt_exceptions.cpp
create mode 100644 src/dynamixel_pan_tilt_exceptions.h
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index f5c48e0..a682741 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -1,10 +1,10 @@
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)
diff --git a/src/dynamixel_pan_tilt.cpp b/src/dynamixel_pan_tilt.cpp
index 589940b..3d18207 100644
--- a/src/dynamixel_pan_tilt.cpp
+++ b/src/dynamixel_pan_tilt.cpp
@@ -1,4 +1,4 @@
-// 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.
//
@@ -29,13 +29,10 @@
///---------------------------------------------------Constructors & Destructors
-CDynamixel_Pan_Tilt::CDynamixel_Pan_Tilt(std::string& name_pan_tilt, CDynamixelServer *dyn_server_pan_tilt, unsigned char dev_id_pan, unsigned char dev_id_tilt, dyn_version_t version_servos) : exiting("exiting"), torque_moving("torque_moving")
+CDynamixel_Pan_Tilt::CDynamixel_Pan_Tilt(std::string& name_pan_tilt, CDynamixelServer *dyn_server_pan_tilt, unsigned char dev_id_pan, unsigned char dev_id_tilt, dyn_version_t version_servos)
{
this->pan = NULL;
this->tilt = NULL;
- reset_torque_moving_state();
- set_checking_hysteresis_pan(0.0);
- set_checking_hysteresis_tilt(0.0);
this->checking_hysteresis_slope.pan = 0.0;
this->checking_hysteresis_slope.tilt = 0.0;
this->checking_hysteresis_offset.pan = 0.0;
@@ -48,17 +45,15 @@ CDynamixel_Pan_Tilt::CDynamixel_Pan_Tilt(std::string& name_pan_tilt, CDynamixelS
this->tilt = new CDynamixelMotor(name, dyn_server_pan_tilt, dev_id_tilt, version_servos);
this->event_server=CEventServer::instance();
+ this->exiting = name_pan_tilt + "_exiting";
this->event_server->create_event(this->exiting);
+ this->torque_moving = name_pan_tilt + "_torque_moving";
this->event_server->create_event(this->torque_moving);
if (this->event_server->event_is_set(this->exiting))
- {
this->event_server->reset_event(this->exiting);
- }
if (this->event_server->event_is_set(this->torque_moving))
- {
this->event_server->reset_event(this->torque_moving);
- }
int res;
@@ -67,6 +62,9 @@ CDynamixel_Pan_Tilt::CDynamixel_Pan_Tilt(std::string& name_pan_tilt, CDynamixelS
perror("Mutex initialization failed");
exit(EXIT_FAILURE);
}
+ reset_torque_moving_state();
+ set_checking_hysteresis_pan(0.0);
+ set_checking_hysteresis_tilt(0.0);
res = pthread_create(&(this->checking_thread), NULL, checking_angle_limits, (void *)this);
if (res != 0)
{
@@ -83,13 +81,13 @@ CDynamixel_Pan_Tilt::CDynamixel_Pan_Tilt(std::string& name_pan_tilt, CDynamixelS
if(this->pan!=NULL)
{
delete this->pan;
- this->pan=NULL;
}
if(this->tilt!=NULL)
{
delete this->tilt;
- this->tilt=NULL;
}
+ this->pan=NULL;
+ this->tilt=NULL;
throw;
}
@@ -106,7 +104,8 @@ CDynamixel_Pan_Tilt::~CDynamixel_Pan_Tilt()
int res;
void *thread_res;
- this->event_server->set_event(this->exiting);
+ if (!this->event_server->event_is_set(this->exiting))
+ this->event_server->set_event(this->exiting);
res = pthread_join(this->checking_thread, &thread_res);
if (res != 0)
{
@@ -115,6 +114,16 @@ CDynamixel_Pan_Tilt::~CDynamixel_Pan_Tilt()
}
pthread_mutex_destroy(&(this->mut));
+ if(this->exiting != "");
+ {
+ this->event_server->delete_event(this->exiting);
+ this->exiting = "";
+ }
+ if(this->torque_moving != "");
+ {
+ this->event_server->delete_event(this->torque_moving);
+ this->torque_moving = "";
+ }
}catch(CException &e){
/* do nothing */
}
@@ -137,35 +146,42 @@ CDynamixel_Pan_Tilt::~CDynamixel_Pan_Tilt()
void CDynamixel_Pan_Tilt::load_config(Dynamixel_pan_tilt_config &config)
{
- try
+ if (this->pan!=NULL && this->tilt!=NULL)
{
- this->pan->load_config(config.pan);
- this->tilt->load_config(config.tilt);
-
- this->config.pan.max_angle = config.pan.max_angle;
- this->config.pan.min_angle = config.pan.min_angle;
- this->config.pan.max_temperature = config.pan.max_temperature;
- this->config.pan.max_voltage = config.pan.max_voltage;
- this->config.pan.min_voltage = config.pan.min_voltage;
- this->config.pan.max_torque = config.pan.max_torque;
- this->config.pan.punch = config.pan.punch;
-
- this->config.tilt.max_angle = config.tilt.max_angle;
- this->config.tilt.min_angle = config.tilt.min_angle;
- this->config.tilt.max_temperature = config.tilt.max_temperature;
- this->config.tilt.max_voltage = config.tilt.max_voltage;
- this->config.tilt.min_voltage = config.tilt.min_voltage;
- this->config.tilt.max_torque = config.tilt.max_torque;
- this->config.tilt.punch = config.tilt.punch;
- }
- catch (CException &e)
+ try
+ {
+ this->pan->load_config(config.pan);
+ this->tilt->load_config(config.tilt);
+
+ this->config.pan.max_angle = config.pan.max_angle;
+ this->config.pan.min_angle = config.pan.min_angle;
+ this->config.pan.max_temperature = config.pan.max_temperature;
+ this->config.pan.max_voltage = config.pan.max_voltage;
+ this->config.pan.min_voltage = config.pan.min_voltage;
+ this->config.pan.max_torque = config.pan.max_torque;
+ this->config.pan.punch = config.pan.punch;
+
+ this->config.tilt.max_angle = config.tilt.max_angle;
+ this->config.tilt.min_angle = config.tilt.min_angle;
+ this->config.tilt.max_temperature = config.tilt.max_temperature;
+ this->config.tilt.max_voltage = config.tilt.max_voltage;
+ this->config.tilt.min_voltage = config.tilt.min_voltage;
+ this->config.tilt.max_torque = config.tilt.max_torque;
+ this->config.tilt.punch = config.tilt.punch;
+ }
+ catch (CException &e)
+ {
+ actualice_config();
+ get_pan_tilt_config(config);
+ std::cout << "Loading config exception: " << e.what() << std::endl;
+ throw CDynamixel_Pan_TiltException(_HERE_, "The dynamixel pan&tilt config hasn't be loaded properly.");
+ }
+ }
+ else
{
- actualice_config();
get_pan_tilt_config(config);
+ throw CDynamixel_Pan_TiltException(_HERE_,"The dynamixel pan&tilt is not properly configured. Config not loaded");
}
-
-
-
}
#ifdef _HAVE_XSD
@@ -176,160 +192,170 @@ void CDynamixel_Pan_Tilt::load_config(std::string &filename)
Dynamixel_pan_tilt_pid pid;
struct stat buffer;
- if(stat(filename.c_str(),&buffer)==0)
+ if (this->pan!=NULL && this->tilt!=NULL)
{
- // try to open the specified file
- try{
- std::auto_ptr<dynamixel_pan_tilt_config_t> cfg(dynamixel_pan_tilt_config(filename.c_str(), xml_schema::flags::dont_validate));
- // configure the parameters of the controller
- if (cfg->alarm_shtdwn().present())
- {
- this->pan->set_turn_off_alarms(cfg->alarm_shtdwn().get());
- this->tilt->set_turn_off_alarms(cfg->alarm_shtdwn().get());
- }
- else
- {
- this->pan->set_turn_off_alarms((unsigned int) 5);
- this->tilt->set_turn_off_alarms((unsigned int) 5);
- }
+ if(stat(filename.c_str(),&buffer)==0)
+ {
+ // try to open the specified file
+ try{
+ std::auto_ptr<dynamixel_pan_tilt_config_t> cfg(dynamixel_pan_tilt_config(filename.c_str(), xml_schema::flags::dont_validate));
+ // configure the parameters of the controller
+ if (cfg->alarm_shtdwn().present())
+ {
+ this->pan->set_turn_off_alarms(cfg->alarm_shtdwn().get());
+ this->tilt->set_turn_off_alarms(cfg->alarm_shtdwn().get());
+ }
+ else
+ {
+ this->pan->set_turn_off_alarms((unsigned int) 5);
+ this->tilt->set_turn_off_alarms((unsigned int) 5);
+ }
- dynamixel_pan_tilt_config_t::min_angle_const_iterator min_angle (cfg->min_angle().begin());
- dynamixel_pan_tilt_config_t::max_angle_const_iterator max_angle (cfg->max_angle().begin());
- this->pan->set_position_range(*min_angle, *max_angle);
+ dynamixel_pan_tilt_config_t::min_angle_const_iterator min_angle (cfg->min_angle().begin());
+ dynamixel_pan_tilt_config_t::max_angle_const_iterator max_angle (cfg->max_angle().begin());
+ this->pan->set_position_range(*min_angle, *max_angle);
- min_angle = cfg->min_angle().end();
- max_angle = cfg->max_angle().end();
- min_angle--;
- max_angle--;
- this->tilt->set_position_range(*min_angle, *max_angle);
+ min_angle = cfg->min_angle().end();
+ max_angle = cfg->max_angle().end();
+ min_angle--;
+ max_angle--;
+ this->tilt->set_position_range(*min_angle, *max_angle);
- if (cfg->temp_limit().present())
- {
- this->pan->set_temperature_limit(cfg->temp_limit().get());
- this->tilt->set_temperature_limit(cfg->temp_limit().get());
- }
- else
- {
- this->pan->set_temperature_limit(85);
- this->tilt->set_temperature_limit(85);
- }
-
- this->pan->set_voltage_limits(cfg->min_voltage(),cfg->max_voltage());
- this->tilt->set_voltage_limits(cfg->min_voltage(),cfg->max_voltage());
+ if (cfg->temp_limit().present())
+ {
+ this->pan->set_temperature_limit(cfg->temp_limit().get());
+ this->tilt->set_temperature_limit(cfg->temp_limit().get());
+ }
+ else
+ {
+ this->pan->set_temperature_limit(85);
+ this->tilt->set_temperature_limit(85);
+ }
+
+ this->pan->set_voltage_limits(cfg->min_voltage(),cfg->max_voltage());
+ this->tilt->set_voltage_limits(cfg->min_voltage(),cfg->max_voltage());
- if(this->info.pan.pid_control)
- {
- dynamixel_pan_tilt_config_t::kp_const_iterator kp (cfg->kp().begin());
- pid.pan.p = *kp;
- dynamixel_pan_tilt_config_t::ki_const_iterator ki (cfg->ki().begin());
- pid.pan.i = *ki;
- dynamixel_pan_tilt_config_t::kd_const_iterator kd (cfg->kd().begin());
- pid.pan.d = *kd;
- this->pan->set_pid_control(pid.pan);
- }
- else
- {
- dynamixel_pan_tilt_config_t::cw_comp_margin_const_iterator cw_comp_margin (cfg->cw_comp_margin().begin());
- compliance.pan.cw_compliance_margin = *cw_comp_margin;
- dynamixel_pan_tilt_config_t::ccw_comp_margin_const_iterator ccw_comp_margin (cfg->ccw_comp_margin().begin());
- compliance.pan.ccw_compliance_margin = *ccw_comp_margin;
- dynamixel_pan_tilt_config_t::cw_comp_slope_const_iterator cw_comp_slope (cfg->cw_comp_slope().begin());
- compliance.pan.cw_compliance_slope = *cw_comp_slope;
- dynamixel_pan_tilt_config_t::ccw_comp_slope_const_iterator ccw_comp_slope (cfg->ccw_comp_slope().begin());
- compliance.pan.ccw_compliance_slope = *ccw_comp_slope;
- this->pan->set_compliance_control(compliance.pan);
- }
+ if(this->info.pan.pid_control)
+ {
+ dynamixel_pan_tilt_config_t::kp_const_iterator kp (cfg->kp().begin());
+ pid.pan.p = *kp;
+ dynamixel_pan_tilt_config_t::ki_const_iterator ki (cfg->ki().begin());
+ pid.pan.i = *ki;
+ dynamixel_pan_tilt_config_t::kd_const_iterator kd (cfg->kd().begin());
+ pid.pan.d = *kd;
+ this->pan->set_pid_control(pid.pan);
+ }
+ else
+ {
+ dynamixel_pan_tilt_config_t::cw_comp_margin_const_iterator cw_comp_margin (cfg->cw_comp_margin().begin());
+ compliance.pan.cw_compliance_margin = *cw_comp_margin;
+ dynamixel_pan_tilt_config_t::ccw_comp_margin_const_iterator ccw_comp_margin (cfg->ccw_comp_margin().begin());
+ compliance.pan.ccw_compliance_margin = *ccw_comp_margin;
+ dynamixel_pan_tilt_config_t::cw_comp_slope_const_iterator cw_comp_slope (cfg->cw_comp_slope().begin());
+ compliance.pan.cw_compliance_slope = *cw_comp_slope;
+ dynamixel_pan_tilt_config_t::ccw_comp_slope_const_iterator ccw_comp_slope (cfg->ccw_comp_slope().begin());
+ compliance.pan.ccw_compliance_slope = *ccw_comp_slope;
+ this->pan->set_compliance_control(compliance.pan);
+ }
- if(this->info.tilt.pid_control)
- {
- dynamixel_pan_tilt_config_t::kp_const_iterator kp (cfg->kp().end());
- kp--;
- pid.tilt.p = *kp;
- dynamixel_pan_tilt_config_t::ki_const_iterator ki (cfg->ki().end());
- ki--;
- pid.tilt.i = *ki;
- dynamixel_pan_tilt_config_t::kd_const_iterator kd (cfg->kd().end());
- kd--;
- pid.tilt.d = *kd;
- this->tilt->set_pid_control(pid.tilt);
- }
- else
- {
- dynamixel_pan_tilt_config_t::cw_comp_margin_const_iterator cw_comp_margin (cfg->cw_comp_margin().end());
- cw_comp_margin--;
- compliance.tilt.cw_compliance_margin = *cw_comp_margin;
- dynamixel_pan_tilt_config_t::ccw_comp_margin_const_iterator ccw_comp_margin (cfg->ccw_comp_margin().end());
- ccw_comp_margin--;
- compliance.tilt.ccw_compliance_margin = *ccw_comp_margin;
- dynamixel_pan_tilt_config_t::cw_comp_slope_const_iterator cw_comp_slope (cfg->cw_comp_slope().end());
- cw_comp_slope--;
- compliance.tilt.cw_compliance_slope = *cw_comp_slope;
- dynamixel_pan_tilt_config_t::ccw_comp_slope_const_iterator ccw_comp_slope (cfg->ccw_comp_slope().end());
- ccw_comp_slope--;
- compliance.tilt.ccw_compliance_slope = *ccw_comp_slope;
- this->tilt->set_compliance_control(compliance.tilt);
- }
+ if(this->info.tilt.pid_control)
+ {
+ dynamixel_pan_tilt_config_t::kp_const_iterator kp (cfg->kp().end());
+ kp--;
+ pid.tilt.p = *kp;
+ dynamixel_pan_tilt_config_t::ki_const_iterator ki (cfg->ki().end());
+ ki--;
+ pid.tilt.i = *ki;
+ dynamixel_pan_tilt_config_t::kd_const_iterator kd (cfg->kd().end());
+ kd--;
+ pid.tilt.d = *kd;
+ this->tilt->set_pid_control(pid.tilt);
+ }
+ else
+ {
+ dynamixel_pan_tilt_config_t::cw_comp_margin_const_iterator cw_comp_margin (cfg->cw_comp_margin().end());
+ cw_comp_margin--;
+ compliance.tilt.cw_compliance_margin = *cw_comp_margin;
+ dynamixel_pan_tilt_config_t::ccw_comp_margin_const_iterator ccw_comp_margin (cfg->ccw_comp_margin().end());
+ ccw_comp_margin--;
+ compliance.tilt.ccw_compliance_margin = *ccw_comp_margin;
+ dynamixel_pan_tilt_config_t::cw_comp_slope_const_iterator cw_comp_slope (cfg->cw_comp_slope().end());
+ cw_comp_slope--;
+ compliance.tilt.cw_compliance_slope = *cw_comp_slope;
+ dynamixel_pan_tilt_config_t::ccw_comp_slope_const_iterator ccw_comp_slope (cfg->ccw_comp_slope().end());
+ ccw_comp_slope--;
+ compliance.tilt.ccw_compliance_slope = *ccw_comp_slope;
+ this->tilt->set_compliance_control(compliance.tilt);
+ }
- dynamixel_pan_tilt_config_t::punch_const_iterator punch (cfg->punch().begin());
- this->pan->set_punch(*punch);
- punch = cfg->punch().end();
- punch--;
- this->tilt->set_punch(*punch);
+ dynamixel_pan_tilt_config_t::punch_const_iterator punch (cfg->punch().begin());
+ this->pan->set_punch(*punch);
+ punch = cfg->punch().end();
+ punch--;
+ this->tilt->set_punch(*punch);
- if (cfg->max_torque().present())
- {
- this->pan->set_max_torque(cfg->max_torque().get());
- this->pan->set_limit_torque(cfg->max_torque().get());
- this->tilt->set_max_torque(cfg->max_torque().get());
- this->tilt->set_limit_torque(cfg->max_torque().get());
- }
- else
- {
- this->pan->set_max_torque(100.0);
- this->pan->set_limit_torque(100.0);
- this->tilt->set_max_torque(100.0);
- this->tilt->set_limit_torque(100.0);
- }
+ if (cfg->max_torque().size() > 0)
+ {
+ dynamixel_pan_tilt_config_t::max_torque_const_iterator max_torque (cfg->max_torque().begin());
+ this->pan->set_max_torque(*max_torque);
+ this->pan->set_limit_torque(*max_torque);
+ max_torque = cfg->max_torque().end();
+ max_torque--;
+ this->tilt->set_max_torque(*max_torque);
+ this->tilt->set_limit_torque(*max_torque);
+ }
+ else
+ {
+ this->pan->set_max_torque(50.0);
+ this->pan->set_limit_torque(50.0);
+ this->tilt->set_max_torque(50.0);
+ this->tilt->set_limit_torque(50.0);
+ }
- if (cfg->check_hyst_offset().size() > 0)
- {
- dynamixel_pan_tilt_config_t::check_hyst_offset_const_iterator check_hyst_offset (cfg->check_hyst_offset().begin());
- this->checking_hysteresis_offset.pan = *check_hyst_offset;
- check_hyst_offset = cfg->check_hyst_offset().end();
- check_hyst_offset--;
- this->checking_hysteresis_offset.tilt = *check_hyst_offset;
- }
- else
- {
- this->checking_hysteresis_offset.pan = 2.404867;
- this->checking_hysteresis_offset.tilt = 2.404867;
- }
+ if (cfg->check_hyst_offset().size() > 0)
+ {
+ dynamixel_pan_tilt_config_t::check_hyst_offset_const_iterator check_hyst_offset (cfg->check_hyst_offset().begin());
+ this->checking_hysteresis_offset.pan = *check_hyst_offset;
+ check_hyst_offset = cfg->check_hyst_offset().end();
+ check_hyst_offset--;
+ this->checking_hysteresis_offset.tilt = *check_hyst_offset;
+ }
+ else
+ {
+ this->checking_hysteresis_offset.pan = 2.404867;
+ this->checking_hysteresis_offset.tilt = 2.404867;
+ }
- if (cfg->check_hyst_slope().size() > 0)
- {
- dynamixel_pan_tilt_config_t::check_hyst_slope_const_iterator check_hyst_slope (cfg->check_hyst_slope().begin());
- this->checking_hysteresis_slope.pan = *check_hyst_slope;
- check_hyst_slope = cfg->check_hyst_slope().end();
- check_hyst_slope--;
- this->checking_hysteresis_slope.tilt = *check_hyst_slope;
+ if (cfg->check_hyst_slope().size() > 0)
+ {
+ dynamixel_pan_tilt_config_t::check_hyst_slope_const_iterator check_hyst_slope (cfg->check_hyst_slope().begin());
+ this->checking_hysteresis_slope.pan = *check_hyst_slope;
+ check_hyst_slope = cfg->check_hyst_slope().end();
+ check_hyst_slope--;
+ this->checking_hysteresis_slope.tilt = *check_hyst_slope;
+ }
+ else
+ {
+ this->checking_hysteresis_slope.pan = 0.20688;
+ this->checking_hysteresis_slope.tilt = 0.20688;
+ }
+
+ actualice_parameters();
}
- else
+ catch (const xml_schema::exception& e)
{
- this->checking_hysteresis_slope.pan = 0.20688;
- this->checking_hysteresis_slope.tilt = 0.20688;
- }
-
- actualice_parameters();
- }catch (const xml_schema::exception& e){
- std::ostringstream os;
- os << e;
- /* handle exceptions */
- throw CDynamixelMotorException(_HERE_,os.str());
- }
+ std::ostringstream os;
+ os << "loading config exception: " << e;
+ /* handle exceptions */
+ throw CDynamixelMotorException(_HERE_,os.str());
+ }
+ }
+ else
+ throw CDynamixel_Pan_TiltException(_HERE_,"The configuration file does not exist");
}
else
- throw CDynamixelMotorException(_HERE_,"The configuration file does not exist");
+ throw CDynamixel_Pan_TiltException(_HERE_,"The dynamixel pan&tilt is not properly configured. Configuration not loaded");
}
void CDynamixel_Pan_Tilt::save_config(std::string &filename)
@@ -408,103 +434,124 @@ void CDynamixel_Pan_Tilt::save_config(std::string &filename)
void CDynamixel_Pan_Tilt::move_absolute_angle(Dynamixel_pan_tilt_data &angle, Dynamixel_pan_tilt_data &speed)
{
+
move_absolute_angle_pan(angle.pan, speed.pan);
move_absolute_angle_tilt(angle.tilt, speed.tilt);
}
void CDynamixel_Pan_Tilt::move_absolute_angle_pan(double &angle, double &speed)
{
- try
+ if (this->pan!=NULL)
{
- if (this->mode.pan == angle_ctrl)
- {//if it is on angle mode it checks the normal limits and moves
- if (this->config.pan.min_angle >= angle)
- angle = this->config.pan.min_angle + 0.05;//on the exact limit it gives an error beacuse the conversion between angle and the word
- else if (this->config.pan.max_angle <= angle)
- angle = this->config.pan.max_angle - 0.05;//on the exact limit it gives an error beacuse the conversion between angle and the word
-
- if (fabs(speed) > this->info.pan.max_speed*this->config.pan.max_torque/100)
- speed = this->info.pan.max_speed*this->config.pan.max_torque/100;
-
- set_moving_state_pan(stopped);
- this->pan->move_absolute_angle(angle, speed);
- }
- else
- {//if not, it checks the configuration limits and actualice the normal limits and the mode
- if (this->min_angle.pan >= angle)
- angle = this->min_angle.pan + 0.05;//on the exact limit it gives an error beacuse the conversion between angle and the word
- else if (this->max_angle.pan <= angle)
- angle = this->max_angle.pan - 0.05;//on the exact limit it gives an error beacuse the conversion between angle and the word
-
- if (fabs(speed) > this->info.pan.max_speed*this->config.pan.max_torque/100)
- speed = this->info.pan.max_speed*this->config.pan.max_torque/100;
-
- this->pan->set_position_range(this->min_angle.pan, this->max_angle.pan);
+ try
+ {
+ if (this->mode.pan == angle_ctrl)
+ {//if it is on angle mode it checks the normal limits and moves
+ if (this->config.pan.min_angle >= angle)
+ angle = this->config.pan.min_angle + 0.05;//on the exact limit it gives an error beacuse the conversion between angle and the word
+ else if (this->config.pan.max_angle <= angle)
+ angle = this->config.pan.max_angle - 0.05;//on the exact limit it gives an error beacuse the conversion between angle and the word
+
+ if (fabs(speed) > this->info.pan.max_speed*this->config.pan.max_torque/100)
+ speed = this->info.pan.max_speed*this->config.pan.max_torque/100;
+
+ set_moving_state_pan(stopped);
+ this->pan->move_absolute_angle(angle, speed);
+ }
+ else
+ {//if not, it checks the configuration limits and actualice the normal limits and the mode
+ if (this->min_angle.pan >= angle)
+ angle = this->min_angle.pan + 0.05;//on the exact limit it gives an error beacuse the conversion between angle and the word
+ else if (this->max_angle.pan <= angle)
+ angle = this->max_angle.pan - 0.05;//on the exact limit it gives an error beacuse the conversion between angle and the word
+
+ if (fabs(speed) > this->info.pan.max_speed*this->config.pan.max_torque/100)
+ speed = this->info.pan.max_speed*this->config.pan.max_torque/100;
+
+ set_moving_state_pan(stopped);
+ this->pan->set_position_range(this->min_angle.pan, this->max_angle.pan);
+ this->pan->move_absolute_angle(angle, speed);
+ this->mode.pan = angle_ctrl;
+ this->config.pan.min_angle = this->min_angle.pan;
+ this->config.pan.max_angle = this->max_angle.pan;
+ }
- set_moving_state_pan(stopped);
- this->pan->move_absolute_angle(angle, speed);
-
- this->mode.pan = this->pan->get_control_mode();
- this->pan->get_position_range(&this->config.pan.min_angle, &this->config.pan.max_angle);
+ if (this->event_server->event_is_set(this->torque_moving))
+ this->event_server->reset_event(this->torque_moving);
+ //set_checking_hysteresis_pan(0.0);
}
-
- if (this->event_server->event_is_set(this->torque_moving))
+ catch(CException &e)
{
- this->event_server->reset_event(this->torque_moving);
+ std::cout << "move pan absolute Exception: ";
+ this->mode.pan = this->pan->get_control_mode();
+ this->pan->get_position_range(&this->config.pan.min_angle, &this->config.pan.max_angle);
+ stop();
+ throw e;
}
- set_checking_hysteresis_pan(0.0);
- }
- catch(CException &e)
+ }
+ else
{
- std::cout << e.what() << std::endl;
+ speed = 0.0;
+ angle = 0.0;
+ throw CDynamixel_Pan_TiltException(_HERE_,"The dynamixel pan servo is not properly configured. Not moving");
}
}
void CDynamixel_Pan_Tilt::move_absolute_angle_tilt(double &angle, double &speed)
{
- try
+ if (this->tilt!=NULL)
{
- if (this->mode.tilt == angle_ctrl)
- {//if it is on angle mode it checks the normal limits and moves
- if (this->config.tilt.min_angle >= angle)
- angle = this->config.tilt.min_angle + 0.05;//on the exact limit it gives an error beacuse the conversion between angle and the word
- else if (this->config.tilt.max_angle <= angle)
- angle = this->config.tilt.max_angle - 0.05;//on the exact limit it gives an error beacuse the conversion between angle and the word
-
- if (fabs(speed) > this->info.tilt.max_speed*this->config.tilt.max_torque/100)
- speed = this->info.tilt.max_speed*this->config.tilt.max_torque/100;
-
- set_moving_state_tilt(stopped);
- this->tilt->move_absolute_angle(angle, speed);
- }
- else
- {//if not, it checks the configuration limits and actualice the normal limits and the mode
- if (this->min_angle.tilt >= angle)
- angle = this->min_angle.tilt + 0.05;//on the exact limit it gives an error beacuse the conversion between angle and the word
- else if (this->max_angle.tilt <= angle)
- angle = this->max_angle.tilt - 0.05;//on the exact limit it gives an error beacuse the conversion between angle and the word
-
- if (fabs(speed) > this->info.tilt.max_speed*this->config.tilt.max_torque/100)
- speed = this->info.tilt.max_speed*this->config.tilt.max_torque/100;
-
- this->tilt->set_position_range(this->min_angle.tilt, this->max_angle.tilt);
+ try
+ {
+ if (this->mode.tilt == angle_ctrl)
+ {//if it is on angle mode it checks the normal limits and moves
+ if (this->config.tilt.min_angle >= angle)
+ angle = this->config.tilt.min_angle + 0.05;//on the exact limit it gives an error beacuse the conversion between angle and the word
+ else if (this->config.tilt.max_angle <= angle)
+ angle = this->config.tilt.max_angle - 0.05;//on the exact limit it gives an error beacuse the conversion between angle and the word
+
+ if (fabs(speed) > this->info.tilt.max_speed*this->config.tilt.max_torque/100)
+ speed = this->info.tilt.max_speed*this->config.tilt.max_torque/100;
+
+ set_moving_state_tilt(stopped);
+ this->tilt->move_absolute_angle(angle, speed);
+ }
+ else
+ {//if not, it checks the configuration limits and actualice the normal limits and the mode
+ if (this->min_angle.tilt >= angle)
+ angle = this->min_angle.tilt + 0.05;//on the exact limit it gives an error beacuse the conversion between angle and the word
+ else if (this->max_angle.tilt <= angle)
+ angle = this->max_angle.tilt - 0.05;//on the exact limit it gives an error beacuse the conversion between angle and the word
+
+ if (fabs(speed) > this->info.tilt.max_speed*this->config.tilt.max_torque/100)
+ speed = this->info.tilt.max_speed*this->config.tilt.max_torque/100;
+
+ set_moving_state_tilt(stopped);
+ this->tilt->set_position_range(this->min_angle.tilt, this->max_angle.tilt);
+ this->tilt->move_absolute_angle(angle, speed);
+ this->mode.tilt = angle_ctrl;
+ this->config.tilt.min_angle = this->min_angle.tilt;
+ this->config.tilt.max_angle = this->max_angle.tilt;
+ }
- set_moving_state_tilt(stopped);
- this->tilt->move_absolute_angle(angle, speed);
-
- this->mode.tilt = this->tilt->get_control_mode();
- this->tilt->get_position_range(&this->config.tilt.min_angle, &this->config.tilt.max_angle);
+ if (this->event_server->event_is_set(this->torque_moving))
+ this->event_server->reset_event(this->torque_moving);
+ //set_checking_hysteresis_tilt(0.0);
}
-
- if (this->event_server->event_is_set(this->torque_moving))
+ catch(CException &e)
{
- this->event_server->reset_event(this->torque_moving);
+ std::cout << "move tilt absolute Exception: ";
+ this->mode.tilt = this->tilt->get_control_mode();
+ this->tilt->get_position_range(&this->config.tilt.min_angle, &this->config.tilt.max_angle);
+ stop();
+ throw e;
}
- set_checking_hysteresis_tilt(0.0);
- }
- catch(CException &e)
+ }
+ else
{
- std::cout << e.what() << std::endl;
+ speed = 0.0;
+ angle = 0.0;
+ throw CDynamixel_Pan_TiltException(_HERE_,"The dynamixel tilt servo is not properly configured. Not moving");
}
}
@@ -516,14 +563,32 @@ void CDynamixel_Pan_Tilt::move_relative_angle(Dynamixel_pan_tilt_data &angle, Dy
void CDynamixel_Pan_Tilt::move_relative_angle_pan(double &angle, double &speed)
{
- angle += this->pan->get_current_angle();
- move_absolute_angle_pan(angle, speed);
+ if (this->pan!=NULL)
+ {
+ angle += this->pan->get_current_angle();
+ move_absolute_angle_pan(angle, speed);
+ }
+ else
+ {
+ angle = 0.0;
+ speed = 0.0;
+ throw CDynamixel_Pan_TiltException(_HERE_,"The dynamixel tilt servo is not properly configured. Not moving");
+ }
}
void CDynamixel_Pan_Tilt::move_relative_angle_tilt(double &angle, double &speed)
{
- angle += this->tilt->get_current_angle();
- move_absolute_angle_tilt(angle, speed);
+ if (this->tilt != NULL)
+ {
+ angle += this->tilt->get_current_angle();
+ move_absolute_angle_tilt(angle, speed);
+ }
+ else
+ {
+ angle = 0.0;
+ speed = 0.0;
+ throw CDynamixel_Pan_TiltException(_HERE_,"The dynamixel tilt servo is not properly configured. Not moving");
+ }
}
void CDynamixel_Pan_Tilt::move_torque(Dynamixel_pan_tilt_data &speed)
@@ -534,10 +599,11 @@ void CDynamixel_Pan_Tilt::move_torque(Dynamixel_pan_tilt_data &speed)
void CDynamixel_Pan_Tilt::move_torque_pan(double &speed)
{
- try
+ if (this->pan!=NULL)
{
if (fabs(speed) > this->config.pan.max_torque)
(speed > 0 ? speed = this->config.pan.max_torque : speed = -(this->config.pan.max_torque));
+
set_checking_hysteresis_pan(calculate_checking_hysteresis_pan(fabs(speed)));
double pos = get_pan_position();
double hyst_aux = 0.0;
@@ -548,16 +614,30 @@ void CDynamixel_Pan_Tilt::move_torque_pan(double &speed)
get_checking_hysteresis_pan(hyst_aux);
if (pos < (this->max_angle.pan - hyst_aux))
{
- this->pan->move_torque(speed);
- set_moving_state_pan(positive_torque);
- if (!this->event_server->event_is_set(this->torque_moving))
- this->event_server->set_event(this->torque_moving);
+ try
+ {
+ set_moving_state_pan(positive_torque);
+ this->pan->move_torque(speed);
+ if (!this->event_server->event_is_set(this->torque_moving))
+ this->event_server->set_event(this->torque_moving);
+ }
+ catch (CException &e)
+ {
+ std::cout << "move pan torque Exception: ";
+ stop();
+ this->mode.pan = this->pan->get_control_mode();
+ this->pan->get_position_range(&this->config.pan.min_angle, &this->config.pan.max_angle);
+ if (!this->event_server->event_is_set(this->torque_moving))
+ this->event_server->set_event(this->torque_moving);
+ throw;
+ }
}
else
{
if (ctrl_mode_changed) ctrl_mode_changed = !ctrl_mode_changed;
double angle = this->max_angle.pan;
double aux_speed = speed*this->info.pan.max_speed/100.0;
+ set_moving_state_pan(stopped);
this->move_absolute_angle_pan(angle, aux_speed);
}
}
@@ -566,45 +646,75 @@ void CDynamixel_Pan_Tilt::move_torque_pan(double &speed)
get_checking_hysteresis_pan(hyst_aux);
if (pos > (this->min_angle.pan + hyst_aux))
{
- this->pan->move_torque(speed);
- set_moving_state_pan(negative_torque);
- if (!this->event_server->event_is_set(this->torque_moving))
- this->event_server->set_event(this->torque_moving);
+ try
+ {
+ set_moving_state_pan(negative_torque);
+ this->pan->move_torque(speed);
+ if (!this->event_server->event_is_set(this->torque_moving))
+ this->event_server->set_event(this->torque_moving);
+ }
+ catch (CException &e)
+ {
+ std::cout << "move pan torque Exception: ";
+ stop();
+ this->mode.pan = this->pan->get_control_mode();
+ this->pan->get_position_range(&this->config.pan.min_angle, &this->config.pan.max_angle);
+ if (!this->event_server->event_is_set(this->torque_moving))
+ this->event_server->set_event(this->torque_moving);
+ throw;
+ }
}
else
{
if (ctrl_mode_changed) ctrl_mode_changed = !ctrl_mode_changed;
double angle = this->min_angle.pan;
double aux_speed = speed*this->info.pan.max_speed/100.0;
+ set_moving_state_pan(stopped);
this->move_absolute_angle_pan(angle, aux_speed);
}
}
else
{
- set_moving_state_pan(stopped);
- if (this->event_server->event_is_set(this->torque_moving))
- this->event_server->reset_event(this->torque_moving);
- this->pan->move_torque(speed);
+ try
+ {
+ set_moving_state_pan(stopped);
+ this->pan->move_torque(speed);
+ if (!this->event_server->event_is_set(this->torque_moving))
+ this->event_server->set_event(this->torque_moving);
+ }
+ catch (CException &e)
+ {
+ std::cout << "move pan torque Exception: ";
+ stop();
+ this->mode.pan = this->pan->get_control_mode();
+ this->pan->get_position_range(&this->config.pan.min_angle, &this->config.pan.max_angle);
+ if (!this->event_server->event_is_set(this->torque_moving))
+ this->event_server->set_event(this->torque_moving);
+ throw;
+ }
}
if (ctrl_mode_changed)
{
- this->mode.pan = this->pan->get_control_mode();
- this->pan->get_position_range(&this->config.pan.min_angle, &this->config.pan.max_angle);
+ this->mode.pan = torque_ctrl;
+ this->config.pan.min_angle = -this->info.pan.center_angle;
+ this->config.pan.max_angle = -this->info.pan.center_angle;
}
- }
- catch (CException &e)
+ }
+ else
{
- std::cout << e.what() << std::endl;
+ speed = 0.0;
+ throw CDynamixel_Pan_TiltException(_HERE_,"The dynamixel pan servo is not properly configured. Not moving");
}
}
void CDynamixel_Pan_Tilt::move_torque_tilt(double &speed)
{
- try
+ if (this->tilt!=NULL)
{
if (fabs(speed) > this->config.tilt.max_torque)
(speed > 0 ? speed = this->config.tilt.max_torque : speed = -(this->config.tilt.max_torque));
+
set_checking_hysteresis_tilt(calculate_checking_hysteresis_tilt(fabs(speed)));
double pos = get_tilt_position();
double hyst_aux = 0.0;
@@ -615,16 +725,30 @@ void CDynamixel_Pan_Tilt::move_torque_tilt(double &speed)
get_checking_hysteresis_tilt(hyst_aux);
if (pos < (this->max_angle.tilt - hyst_aux))
{
- this->tilt->move_torque(speed);
- set_moving_state_tilt(positive_torque);
- if (!this->event_server->event_is_set(this->torque_moving))
- this->event_server->set_event(this->torque_moving);
+ try
+ {
+ set_moving_state_tilt(positive_torque);
+ this->tilt->move_torque(speed);
+ if (!this->event_server->event_is_set(this->torque_moving))
+ this->event_server->set_event(this->torque_moving);
+ }
+ catch (CException &e)
+ {
+ std::cout << "move tilt torque Exception: ";
+ stop();
+ this->mode.tilt = this->tilt->get_control_mode();
+ this->tilt->get_position_range(&this->config.tilt.min_angle, &this->config.tilt.max_angle);
+ if (!this->event_server->event_is_set(this->torque_moving))
+ this->event_server->set_event(this->torque_moving);
+ throw;
+ }
}
else
{
if (ctrl_mode_changed) ctrl_mode_changed = !ctrl_mode_changed;
double angle = this->max_angle.tilt;
double aux_speed = speed*this->info.tilt.max_speed/100.0;
+ set_moving_state_tilt(stopped);
this->move_absolute_angle_tilt(angle, aux_speed);
}
}
@@ -633,35 +757,64 @@ void CDynamixel_Pan_Tilt::move_torque_tilt(double &speed)
get_checking_hysteresis_tilt(hyst_aux);
if (pos > (this->min_angle.tilt + hyst_aux))
{
- this->tilt->move_torque(speed);
- set_moving_state_tilt(negative_torque);
- if (!this->event_server->event_is_set(this->torque_moving))
- this->event_server->set_event(this->torque_moving);
+ try
+ {
+ set_moving_state_tilt(negative_torque);
+ this->tilt->move_torque(speed);
+ if (!this->event_server->event_is_set(this->torque_moving))
+ this->event_server->set_event(this->torque_moving);
+ }
+ catch (CException &e)
+ {
+ std::cout << "move tilt torque Exception: ";
+ stop();
+ this->mode.tilt = this->tilt->get_control_mode();
+ this->tilt->get_position_range(&this->config.tilt.min_angle, &this->config.tilt.max_angle);
+ if (!this->event_server->event_is_set(this->torque_moving))
+ this->event_server->set_event(this->torque_moving);
+ throw;
+ }
}
else
{
if (ctrl_mode_changed) ctrl_mode_changed = !ctrl_mode_changed;
double angle = this->min_angle.tilt;
double aux_speed = speed*this->info.tilt.max_speed/100.0;
+ set_moving_state_tilt(stopped);
this->move_absolute_angle_tilt(angle, aux_speed);
}
}
else
{
- set_moving_state_tilt(stopped);
- if (this->event_server->event_is_set(this->torque_moving))
- this->event_server->reset_event(this->torque_moving);
- this->tilt->move_torque(speed);
+ try
+ {
+ set_moving_state_tilt(stopped);
+ this->tilt->move_torque(speed);
+ if (!this->event_server->event_is_set(this->torque_moving))
+ this->event_server->set_event(this->torque_moving);
+ }
+ catch (CException &e)
+ {
+ std::cout << "move tilt torque Exception: ";
+ stop();
+ this->mode.tilt = this->tilt->get_control_mode();
+ this->tilt->get_position_range(&this->config.tilt.min_angle, &this->config.tilt.max_angle);
+ if (!this->event_server->event_is_set(this->torque_moving))
+ this->event_server->set_event(this->torque_moving);
+ throw;
+ }
}
if (ctrl_mode_changed)
{
- this->mode.tilt = this->tilt->get_control_mode();
- this->tilt->get_position_range(&this->config.tilt.min_angle, &this->config.tilt.max_angle);
+ this->mode.tilt = torque_ctrl;
+ this->config.tilt.min_angle = -this->info.tilt.center_angle;
+ this->config.tilt.max_angle = -this->info.tilt.center_angle;
}
- }
- catch (CException &e)
+ }
+ else
{
- std::cout << e.what() << std::endl;
+ speed = 0.0;
+ throw CDynamixel_Pan_TiltException(_HERE_,"The dynamixel tilt servo is not properly configured. Not moving");
}
}
@@ -759,15 +912,24 @@ void *CDynamixel_Pan_Tilt::checking_angle_limits(void *arg)
void CDynamixel_Pan_Tilt::stop(void)
{
- Dynamixel_pan_tilt_data dat;
- dat.pan = 0.0;
- dat.tilt = 0.0;
- //this->pan->stop();
- //this->tilt->stop();
- move_torque(dat);
- reset_torque_moving_state();
- set_checking_hysteresis_pan(0.0);
- set_checking_hysteresis_tilt(0.0);
+ try
+ {
+ this->pan->stop();
+ this->tilt->stop();
+ reset_torque_moving_state();
+ }
+ catch (CException &e)
+ {
+ std::cout << "stop Exception: ";
+ //stop();
+ throw;
+ }
+ //Dynamixel_pan_tilt_data dat;
+ //dat.pan = 0.0;
+ //dat.tilt = 0.0;
+ //move_torque(dat);
+ //set_checking_hysteresis_pan(0.0);
+ //set_checking_hysteresis_tilt(0.0);
}
@@ -787,17 +949,30 @@ void CDynamixel_Pan_Tilt::actualice_parameters(void)
void CDynamixel_Pan_Tilt::actualice_config(void)
{
- this->pan->get_position_range(&this->config.pan.min_angle, &this->config.pan.max_angle);
- this->config.pan.max_temperature = this->pan->get_temperature_limit();
- this->pan->get_voltage_limits(&this->config.pan.min_voltage, &this->config.pan.max_voltage);
- this->config.pan.max_torque = this->pan->get_max_torque();
- this->config.pan.punch = this->pan->get_punch();
-
- this->tilt->get_position_range(&this->config.tilt.min_angle, &this->config.tilt.max_angle);
- this->config.tilt.max_temperature = this->tilt->get_temperature_limit();
- this->tilt->get_voltage_limits(&this->config.tilt.min_voltage, &this->config.tilt.max_voltage);
- this->config.tilt.max_torque = this->tilt->get_max_torque();
- this->config.tilt.punch = this->tilt->get_punch();
+ if (this->pan!=NULL && this->tilt!=NULL)
+ {
+ try
+ {
+ this->pan->get_position_range(&this->config.pan.min_angle, &this->config.pan.max_angle);
+ this->config.pan.max_temperature = this->pan->get_temperature_limit();
+ this->pan->get_voltage_limits(&this->config.pan.min_voltage, &this->config.pan.max_voltage);
+ this->config.pan.max_torque = this->pan->get_max_torque();
+ this->config.pan.punch = this->pan->get_punch();
+
+ this->tilt->get_position_range(&this->config.tilt.min_angle, &this->config.tilt.max_angle);
+ this->config.tilt.max_temperature = this->tilt->get_temperature_limit();
+ this->tilt->get_voltage_limits(&this->config.tilt.min_voltage, &this->config.tilt.max_voltage);
+ this->config.tilt.max_torque = this->tilt->get_max_torque();
+ this->config.tilt.punch = this->tilt->get_punch();
+ }
+ catch(CDynamixelAlarmException &e)
+ {
+ std::cout << "actualice config Exception: " << e.what() << std::endl;
+ throw;
+ }
+ }
+ else
+ throw CDynamixel_Pan_TiltException(_HERE_,"The dynamixel pan&tilt is not properly configured. Config not actualiced");
}
void CDynamixel_Pan_Tilt::actualice_angle_limits(void)
@@ -810,54 +985,78 @@ void CDynamixel_Pan_Tilt::actualice_angle_limits(void)
void CDynamixel_Pan_Tilt::actualice_control_mode(void)
{
- try
+ if (this->pan!=NULL && this->tilt!=NULL)
{
- this->mode.pan = this->pan->get_control_mode();
- this->mode.tilt = this->tilt->get_control_mode();
+ try
+ {
+ this->mode.pan = this->pan->get_control_mode();
+ this->mode.tilt = this->tilt->get_control_mode();
+ }
+ catch(CDynamixelAlarmException &e)
+ {
+ std::cout << "actualice control mode Exception: " << e.what() << std::endl;
+ throw;
+ }
}
- catch(CDynamixelAlarmException &e)
- {
- std::cout << e.what() << std::endl;
- }
+ else
+ throw CDynamixel_Pan_TiltException(_HERE_,"The dynamixel pan&tilt is not properly configured. Config not actualiced");
}
void CDynamixel_Pan_Tilt::actualice_pid_control(void)
{
- try
+ if (this->pan!=NULL && this->tilt!=NULL)
{
- this->pan->get_pid_control(this->pid.pan);
- this->tilt->get_pid_control(this->pid.tilt);
+ try
+ {
+ this->pan->get_pid_control(this->pid.pan);
+ this->tilt->get_pid_control(this->pid.tilt);
+ }
+ catch(CDynamixelAlarmException &e)
+ {
+ std::cout << "actualice pid control exception: " << e.what() << std::endl;
+ throw;
+ }
}
- catch(CDynamixelAlarmException &e)
- {
- std::cout << e.what() << std::endl;
- }
+ else
+ throw CDynamixel_Pan_TiltException(_HERE_,"The dynamixel pan&tilt is not properly configured. PID control not actualiced");
}
void CDynamixel_Pan_Tilt::actualice_info(void)
{
- try
+ if (this->pan!=NULL && this->tilt!=NULL)
{
- this->pan->get_servo_info(this->info.pan);
- this->tilt->get_servo_info(this->info.tilt);
+ try
+ {
+ this->pan->get_servo_info(this->info.pan);
+ this->tilt->get_servo_info(this->info.tilt);
+ }
+ catch(CDynamixelAlarmException &e)
+ {
+ std::cout << "actualice info exception: " << e.what() << std::endl;
+ throw;
+ }
}
- catch(CDynamixelAlarmException &e)
- {
- std::cout << e.what() << std::endl;
- }
+ else
+ throw CDynamixel_Pan_TiltException(_HERE_,"The dynamixel pan&tilt is not properly configured. Info not actualiced");
}
void CDynamixel_Pan_Tilt::actualice_compliance_control(void)
{
- try
+ if (this->pan!=NULL && this->tilt!=NULL)
{
- this->pan->get_compliance_control(this->compliance.pan);
- this->tilt->get_compliance_control(this->compliance.tilt);
+ try
+ {
+ this->pan->get_compliance_control(this->compliance.pan);
+ this->tilt->get_compliance_control(this->compliance.tilt);
+ }
+ catch(CDynamixelAlarmException &e)
+ {
+ std::cout << "actualice compliance control exception: " << e.what() << std::endl;
+ throw;
+ }
}
- catch(CDynamixelAlarmException &e)
- {
- std::cout << e.what() << std::endl;
- }
+ else
+ throw CDynamixel_Pan_TiltException(_HERE_,"The dynamixel pan&tilt is not properly configured. Compliance control not actualiced");
}
void CDynamixel_Pan_Tilt::default_parameters(void)
@@ -913,15 +1112,6 @@ void CDynamixel_Pan_Tilt::default_parameters(void)
this->config.tilt.max_torque=0.0;
this->min_angle.tilt=0.0;
this->max_angle.tilt=0.0;
-
- if (this->event_server->event_is_set(this->exiting))
- {
- this->event_server->reset_event(this->exiting);
- }
- if (this->event_server->event_is_set(this->torque_moving))
- {
- this->event_server->reset_event(this->torque_moving);
- }
reset_torque_moving_state();
set_checking_hysteresis_pan(0.0);
set_checking_hysteresis_tilt(0.0);
@@ -948,28 +1138,38 @@ Dynamixel_pan_tilt_data CDynamixel_Pan_Tilt::get_position(void)
double CDynamixel_Pan_Tilt::get_pan_position(void)
{
- try
+ if (this->pan!=NULL)
{
- return this->pan->get_current_angle();
- }
- catch(CException &e)
- {
- std::cout << e.what() << std::endl;
- }
- return 0;
+ try
+ {
+ return this->pan->get_current_angle();
+ }
+ catch(CException &e)
+ {
+ std::cout << "get pan position exception: " << e.what() << std::endl;
+ throw;
+ }
+ }
+ else
+ throw CDynamixel_Pan_TiltException(_HERE_,"The dynamixel pan servo is not properly configured. Position not actualiced");
}
double CDynamixel_Pan_Tilt::get_tilt_position(void)
{
- try
- {
- return this->tilt->get_current_angle();
- }
- catch(CException &e)
+ if (this->tilt!=NULL)
{
- std::cout << e.what() << std::endl;
- }
- return 0;
+ try
+ {
+ return this->tilt->get_current_angle();
+ }
+ catch(CException &e)
+ {
+ std::cout << "get tilt position exception: " << e.what() << std::endl;
+ throw;
+ }
+ }
+ else
+ throw CDynamixel_Pan_TiltException(_HERE_,"The dynamixel tilt servo is not properly configured. Position not actualiced");
}
void CDynamixel_Pan_Tilt::get_compliance_control(Dynamixel_pan_tilt_compliance &compliance)
@@ -979,18 +1179,27 @@ void CDynamixel_Pan_Tilt::get_compliance_control(Dynamixel_pan_tilt_compliance &
void CDynamixel_Pan_Tilt::set_compliance_control(Dynamixel_pan_tilt_compliance &compliance)
{
- try
+ if (this->pan!=NULL && this->tilt!=NULL)
{
- this->pan->set_compliance_control(compliance.pan);
- this->tilt->set_compliance_control(compliance.tilt);
- this->compliance = compliance;
- }
- catch (CException &e)
+ try
+ {
+ this->pan->set_compliance_control(compliance.pan);
+ this->tilt->set_compliance_control(compliance.tilt);
+ this->compliance = compliance;
+ }
+ catch (CException &e)
+ {
+ std::cout << "set compliance control exception: " << e.what() << std::endl;
+ actualice_compliance_control();
+ compliance = this->compliance;
+ throw CDynamixel_Pan_TiltException(_HERE_, "The dynamixel pan&tilt compliance control hasn't been set properly.");
+ }
+ }
+ else
{
- actualice_compliance_control();
compliance = this->compliance;
+ throw CDynamixel_Pan_TiltException(_HERE_,"The dynamixel pan&tilt is not properly configured. Compliance control not set");
}
-
}
void CDynamixel_Pan_Tilt::get_pid_control(Dynamixel_pan_tilt_pid &pid)
@@ -1000,16 +1209,26 @@ void CDynamixel_Pan_Tilt::get_pid_control(Dynamixel_pan_tilt_pid &pid)
void CDynamixel_Pan_Tilt::set_pid_control(Dynamixel_pan_tilt_pid &pid)
{
- try
+ if (this->pan!=NULL && this->tilt!=NULL)
{
- this->pan->set_pid_control(pid.pan);
- this->tilt->set_pid_control(pid.tilt);
- this->pid = pid;
- }
- catch (CException &e)
+ try
+ {
+ this->pan->set_pid_control(pid.pan);
+ this->tilt->set_pid_control(pid.tilt);
+ this->pid = pid;
+ }
+ catch (CException &e)
+ {
+ std::cout << "set pid control exception: " << e.what() << std::endl;
+ actualice_pid_control();
+ pid = this->pid;
+ throw CDynamixel_Pan_TiltException(_HERE_, "The dynamixel pan&tilt PID control hasn't been set properly.");
+ }
+ }
+ else
{
- actualice_pid_control();
pid = this->pid;
+ throw CDynamixel_Pan_TiltException(_HERE_,"The dynamixel pan&tilt is not properly configured. PID control not set");
}
}
@@ -1154,18 +1373,92 @@ double CDynamixel_Pan_Tilt::calculate_checking_hysteresis_tilt(double effort)
Dynamixel_pan_tilt_data CDynamixel_Pan_Tilt::get_current_speed(void)
{
- Dynamixel_pan_tilt_data data;
- data.pan = this->pan->get_current_speed();
- data.tilt = this->tilt->get_current_speed();
- return data;
+ if (this->pan!=NULL && this->tilt!=NULL)
+ {
+ try
+ {
+ Dynamixel_pan_tilt_data data;
+ data.pan = this->pan->get_current_speed();
+ data.tilt = this->tilt->get_current_speed();
+ return data;
+ }
+ catch(CException &e)
+ {
+ std::cout << "get current speed exception: " << e.what() << std::endl;
+ throw;
+ }
+ }
+ else
+ throw CDynamixel_Pan_TiltException(_HERE_,"The dynamixel pan&tilt is not properly configured. Speed not get");
}
Dynamixel_pan_tilt_data CDynamixel_Pan_Tilt::get_current_effort(void)
{
- Dynamixel_pan_tilt_data data;
- data.pan = this->pan->get_current_effort();
- data.tilt = this->tilt->get_current_effort();
- return data;
+ if (this->pan!=NULL && this->tilt!=NULL)
+ {
+ try
+ {
+ Dynamixel_pan_tilt_data data;
+ data.pan = this->pan->get_current_effort();
+ data.tilt = this->tilt->get_current_effort();
+ return data;
+ }
+ catch(CException &e)
+ {
+ std::cout << "get current effort exception: " << e.what() << std::endl;
+ throw;
+ }
+ }
+ else
+ throw CDynamixel_Pan_TiltException(_HERE_,"The dynamixel pan&tilt is not properly configured. Effort not get");
+}
+
+ void CDynamixel_Pan_Tilt::set_max_torque(Dynamixel_pan_tilt_data &torque)
+{
+ if (this->pan!=NULL && this->tilt!=NULL)
+ {
+ try
+ {
+ this->pan->set_max_torque(torque.pan);
+ this->pan->set_limit_torque(torque.pan);
+ this->tilt->set_max_torque(torque.tilt);
+ this->tilt->set_limit_torque(torque.tilt);
+ }
+ catch(CException &e)
+ {
+ std::cout << "set max torque exception: " << e.what() << std::endl;
+ torque.pan = this->pan->get_limit_torque();
+ torque.tilt = this->tilt->get_limit_torque();
+ throw CDynamixel_Pan_TiltException(_HERE_, "The dynamixel pan&tilt max torque hasn't been set properly.");
+ }
+ }
+ else
+ {
+ torque.pan = 0;
+ torque.tilt = 0;
+ throw CDynamixel_Pan_TiltException(_HERE_,"The dynamixel pan&tilt is not properly configured. Max torque not set");
+ }
+}
+
+Dynamixel_pan_tilt_data CDynamixel_Pan_Tilt::get_limit_torque(void)
+{
+ if (this->pan!=NULL && this->tilt!=NULL)
+ {
+ try
+ {
+ Dynamixel_pan_tilt_data data;
+ data.pan = this->pan->get_limit_torque();
+ data.tilt = this->tilt->get_limit_torque();
+ return data;
+ }
+ catch(CException &e)
+ {
+ std::cout << "get limit torque exception: " << e.what() << std::endl;
+ throw;
+ }
+ }
+ else
+ throw CDynamixel_Pan_TiltException(_HERE_,"The dynamixel pan&tilt is not properly configured. Torque limit not get");
}
///------------------------------------------------------Get & Set
@@ -1179,7 +1472,20 @@ std::ostream& operator<< (std::ostream& out, Dynamixel_pan_tilt_data &data)
}
void CDynamixel_Pan_Tilt::print_current_position(void)
{
- print_pan_tilt_data(this->get_position());
+ if (this->pan!=NULL && this->tilt!=NULL)
+ {
+ try
+ {
+ print_pan_tilt_data(this->get_position());
+ }
+ catch(CException &e)
+ {
+ std::cout << "print current position exception: " << e.what() << std::endl;
+ throw;
+ }
+ }
+ else
+ throw CDynamixel_Pan_TiltException(_HERE_,"The dynamixel pan&tilt is not properly configured.");
}
void CDynamixel_Pan_Tilt::print_pan_tilt_data(Dynamixel_pan_tilt_data data)
diff --git a/src/dynamixel_pan_tilt.h b/src/dynamixel_pan_tilt.h
index a3bdfa7..9b4c3a7 100644
--- a/src/dynamixel_pan_tilt.h
+++ b/src/dynamixel_pan_tilt.h
@@ -1,4 +1,4 @@
-// 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.
*
diff --git a/src/dynamixel_pan_tilt_exceptions.cpp b/src/dynamixel_pan_tilt_exceptions.cpp
new file mode 100644
index 0000000..b648633
--- /dev/null
+++ b/src/dynamixel_pan_tilt_exceptions.cpp
@@ -0,0 +1,28 @@
+// 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
diff --git a/src/dynamixel_pan_tilt_exceptions.h b/src/dynamixel_pan_tilt_exceptions.h
new file mode 100644
index 0000000..4c2892d
--- /dev/null
+++ b/src/dynamixel_pan_tilt_exceptions.h
@@ -0,0 +1,69 @@
+// 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
diff --git a/src/examples/CMakeLists.txt b/src/examples/CMakeLists.txt
index e02df34..fd57ded 100644
--- a/src/examples/CMakeLists.txt
+++ b/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)
diff --git a/src/examples/test_dynamixel_pan_tilt.cpp b/src/examples/test_dynamixel_pan_tilt.cpp
index 225426b..30ed6dc 100644
--- a/src/examples/test_dynamixel_pan_tilt.cpp
+++ b/src/examples/test_dynamixel_pan_tilt.cpp
@@ -1,4 +1,4 @@
-// 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)
diff --git a/src/xml/dynamixel_pan_tilt_cfg_file.hxx b/src/xml/dynamixel_pan_tilt_cfg_file.hxx
index 9b7e4ca..947b6f9 100644
--- a/src/xml/dynamixel_pan_tilt_cfg_file.hxx
+++ b/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_;
diff --git a/src/xml/dynamixel_pan_tilt_cfg_file.xsd b/src/xml/dynamixel_pan_tilt_cfg_file.xsd
index 6d1e3e1..7c4a23f 100644
--- a/src/xml/dynamixel_pan_tilt_cfg_file.xsd
+++ b/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>
--
GitLab