From 11d0f9497d18d93f48518d8ba12fb7dfbecfbd8d Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Alejandro=20L=C3=B3pez=20Gestoso?= <alopez@iri.upc.edu>
Date: Mon, 27 Jun 2016 15:17:56 +0000
Subject: [PATCH] dynamixel_pan_tilt driver: Changed the indentation from tabs
to spaces. Solved a minor error on the destructor.
---
src/dynamixel_pan_tilt.cpp | 2591 +++++++++++-----------
src/dynamixel_pan_tilt.h | 874 ++++----
src/examples/test_dynamixel_pan_tilt.cpp | 4 +-
3 files changed, 1735 insertions(+), 1734 deletions(-)
diff --git a/src/dynamixel_pan_tilt.cpp b/src/dynamixel_pan_tilt.cpp
index 1f7e050..54663e3 100644
--- a/src/dynamixel_pan_tilt.cpp
+++ b/src/dynamixel_pan_tilt.cpp
@@ -31,113 +31,114 @@
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;
- this->checking_hysteresis_slope.pan = 0.0;
- this->checking_hysteresis_slope.tilt = 0.0;
- this->checking_hysteresis_offset.pan = 0.0;
- this->checking_hysteresis_offset.tilt = 0.0;
- try
- {
- std::string name = "pan";
- this->pan = new CDynamixelMotor(name, dyn_server_pan_tilt, dev_id_pan, version_servos);
- name = "tilt";
- 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;
- res = pthread_mutex_init(&(this->mut), NULL);
- if (res != 0) {
- 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)
- {
- perror("Thread creation failed");
- exit(EXIT_FAILURE);
- }
-
- actualice_parameters();
- }
- catch(CException &e)
- {
- /* handle exceptions */
- std::cout << e.what() << std::endl;
- if(this->pan!=NULL)
- {
- delete this->pan;
- }
- if(this->tilt!=NULL)
- {
- delete this->tilt;
- }
- this->pan=NULL;
- this->tilt=NULL;
-
- throw;
- }
+ this->pan = NULL;
+ this->tilt = NULL;
+ this->checking_hysteresis_slope.pan = 0.0;
+ this->checking_hysteresis_slope.tilt = 0.0;
+ this->checking_hysteresis_offset.pan = 0.0;
+ this->checking_hysteresis_offset.tilt = 0.0;
+ try
+ {
+ std::string name = "pan";
+ this->pan = new CDynamixelMotor(name, dyn_server_pan_tilt, dev_id_pan, version_servos);
+ name = "tilt";
+ 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;
+ res = pthread_mutex_init(&(this->mut), NULL);
+ if (res != 0) {
+ 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)
+ {
+ perror("Thread creation failed");
+ exit(EXIT_FAILURE);
+ }
+
+ actualice_parameters();
+ }
+ catch(CException &e)
+ {
+ /* handle exceptions */
+ std::cout << "Exception on the constructor." << std::endl;
+ std::cout << e.what() << std::endl;
+ if(this->pan!=NULL)
+ {
+ delete this->pan;
+ }
+ if(this->tilt!=NULL)
+ {
+ delete this->tilt;
+ }
+ this->pan=NULL;
+ this->tilt=NULL;
+
+ throw;
+ }
}
CDynamixel_Pan_Tilt::~CDynamixel_Pan_Tilt()
{
- /* stop the pan-tilt */
- try{
- stop();
- /* disable the motors */
- this->pan->disable();
- this->tilt->disable();
-
- int res;
- void *thread_res;
- 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)
- {
- perror("Thread join failed");
- exit(EXIT_FAILURE);
- }
- 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 */
- }
- if(this->pan!=NULL)
- {
- delete this->pan;
- this->pan=NULL;
- }
- if(this->pan!=NULL)
- {
- delete this->tilt;
- this->tilt=NULL;
- }
- default_parameters();
+ /* stop the pan-tilt */
+ try{
+ stop();
+ /* disable the motors */
+ this->pan->disable();
+ this->tilt->disable();
+
+ int res;
+ void *thread_res;
+ 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)
+ {
+ perror("Thread join failed");
+ exit(EXIT_FAILURE);
+ }
+ 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 */
+ }
+ if(this->pan!=NULL)
+ {
+ delete this->pan;
+ this->pan=NULL;
+ }
+ if(this->pan!=NULL)
+ {
+ delete this->tilt;
+ this->tilt=NULL;
+ }
+ default_parameters();
}
///---------------------------------------------------Constructors & Destructors
@@ -146,334 +147,334 @@ CDynamixel_Pan_Tilt::~CDynamixel_Pan_Tilt()
void CDynamixel_Pan_Tilt::load_config(Dynamixel_pan_tilt_config &config)
{
- if (this->pan!=NULL && this->tilt!=NULL)
- {
- 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
- {
- get_pan_tilt_config(config);
- throw CDynamixel_Pan_TiltException(_HERE_,"The dynamixel pan&tilt is not properly configured. Config not loaded");
- }
+ if (this->pan!=NULL && this->tilt!=NULL)
+ {
+ 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
+ {
+ get_pan_tilt_config(config);
+ throw CDynamixel_Pan_TiltException(_HERE_,"The dynamixel pan&tilt is not properly configured. Config not loaded");
+ }
}
#ifdef _HAVE_XSD
void CDynamixel_Pan_Tilt::load_config(std::string &filename)
{
- Dynamixel_pan_tilt_compliance compliance;
- Dynamixel_pan_tilt_pid pid;
- struct stat buffer;
-
- if (this->pan!=NULL && this->tilt!=NULL)
- {
- 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);
-
- 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(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);
- }
-
-
- 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().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_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();
- }
- catch (const xml_schema::exception& e)
- {
- 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 CDynamixel_Pan_TiltException(_HERE_,"The dynamixel pan&tilt is not properly configured. Configuration not loaded");
+ Dynamixel_pan_tilt_compliance compliance;
+ Dynamixel_pan_tilt_pid pid;
+ struct stat buffer;
+
+ if (this->pan!=NULL && this->tilt!=NULL)
+ {
+ 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);
+
+ 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(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);
+ }
+
+
+ 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().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_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();
+ }
+ catch (const xml_schema::exception& e)
+ {
+ 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 CDynamixel_Pan_TiltException(_HERE_,"The dynamixel pan&tilt is not properly configured. Configuration not loaded");
}
void CDynamixel_Pan_Tilt::read_config(std::string &filename, Dynamixel_pan_tilt_xml_limits &xml_limits)
{
- struct stat buffer;
-
- 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
-
- 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());
- xml_limits.min_angle.pan = *min_angle;
- xml_limits.max_angle.pan = *max_angle;
-
- min_angle = cfg->min_angle().end();
- max_angle = cfg->max_angle().end();
- min_angle--;
- max_angle--;
- xml_limits.min_angle.tilt = *min_angle;
- xml_limits.max_angle.tilt = *max_angle;
-
- if (cfg->max_torque().size() > 0)
- {
- dynamixel_pan_tilt_config_t::max_torque_const_iterator max_torque (cfg->max_torque().begin());
- xml_limits.max_torque.pan = *max_torque;
- max_torque = cfg->max_torque().end();
- max_torque--;
- xml_limits.max_torque.tilt = *max_torque;
- }
- else
- {
- xml_limits.max_torque.pan = 50;
- xml_limits.max_torque.tilt = 50;
- }
- }
- catch (const xml_schema::exception& e)
- {
- 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.");
+ struct stat buffer;
+
+ 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
+
+ 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());
+ xml_limits.min_angle.pan = *min_angle;
+ xml_limits.max_angle.pan = *max_angle;
+
+ min_angle = cfg->min_angle().end();
+ max_angle = cfg->max_angle().end();
+ min_angle--;
+ max_angle--;
+ xml_limits.min_angle.tilt = *min_angle;
+ xml_limits.max_angle.tilt = *max_angle;
+
+ if (cfg->max_torque().size() > 0)
+ {
+ dynamixel_pan_tilt_config_t::max_torque_const_iterator max_torque (cfg->max_torque().begin());
+ xml_limits.max_torque.pan = *max_torque;
+ max_torque = cfg->max_torque().end();
+ max_torque--;
+ xml_limits.max_torque.tilt = *max_torque;
+ }
+ else
+ {
+ xml_limits.max_torque.pan = 50;
+ xml_limits.max_torque.tilt = 50;
+ }
+ }
+ catch (const xml_schema::exception& e)
+ {
+ 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.");
}
void CDynamixel_Pan_Tilt::save_config(std::string &filename)
{
- /*xml_schema::namespace_infomap map;
- actualice_parameters();
-
- unsigned char c[] = {this->compliance.pan.cw_compliance_margin, this->compliance.tilt.cw_compliance_margin};
- std::vector<unsigned char> cw_compliance_margin(c, c + sizeof(c) / sizeof(unsigned char));
- c[0] = this->compliance.pan.ccw_compliance_margin;
- c[1] = this->compliance.tilt.ccw_compliance_margin;
- std::vector<unsigned char> ccw_compliance_margin(c, c + sizeof(c) / sizeof(unsigned char));
- c[0] = this->compliance.pan.cw_compliance_slope;
- c[1] = this->compliance.tilt.cw_compliance_slope;
- std::vector<unsigned char> cw_compliance_slope(c, c + sizeof(c) / sizeof(unsigned char));
- c[0] = this->compliance.pan.ccw_compliance_slope;
- c[1] = this->compliance.tilt.ccw_compliance_slope;
- std::vector<unsigned char> ccw_compliance_slope(c, c + sizeof(c) / sizeof(unsigned char));
- unsigned short int i[] ={this->config.pan.punch, this->config.tilt.punch};
- std::vector<unsigned short int> punch(i, i + sizeof(i) / sizeof(unsigned short int));
- c[0] = this->pid.pan.p;
- c[1] = this->pid.tilt.p;
- std::vector<unsigned char> kp(c, c + sizeof(c) / sizeof(unsigned char));
- c[0] = this->pid.pan.i;
- c[1] = this->pid.tilt.i;
- std::vector<unsigned char> ki(c, c + sizeof(c) / sizeof(unsigned char));
- c[0] = this->pid.pan.d;
- c[1] = this->pid.tilt.d;
- std::vector<unsigned char> kd(c, c + sizeof(c) / sizeof(unsigned char));
- double d[] = {this->min_angle.pan, this->min_angle.tilt};
- std::vector<double> min_angles(d, d + sizeof(d) / sizeof(double));
- d[0] = this->max_angle.pan;
- d[1] = this->max_angle.tilt;
- std::vector<double> max_angles(d, d + sizeof(d) / sizeof(double));
- d[0] = this->checking_hysteresis_offset.pan;
- d[1] = this->checking_hysteresis_offset.tilt;
- std::vector<double> checking_hyst_offset(d, d + sizeof(d) / sizeof(double));
- d[0] = this->checking_hysteresis_slope.pan;
- d[1] = this->checking_hysteresis_slope.tilt;
- std::vector<double> checking_hyst_slope(d, d + sizeof(d) / sizeof(double));
-
- try{
- std::ofstream output_file(filename.c_str(),std::ios_base::out);
-
- dynamixel_pan_tilt_config_t dyn_cfg((int)this->pan->get_turn_off_alarms(),
- this->config.pan.max_temperature,
- this->config.pan.max_voltage,
- this->config.pan.min_voltage,
- this->config.pan.max_torque,
- cw_compliance_margin,
- ccw_compliance_margin,
- cw_compliance_slope,
- ccw_compliance_slope,
- punch,
- kp,
- ki,
- kd,
- min_angles,
- max_angles,
- checking_hyst_offset,
- checking_hyst_slope);
- map[""].name="";
- map[""].schema="dynamixel_pan_tilt_cfg_file.xsd";
- dynamixel_pan_tilt_config(output_file,dyn_cfg,map);
- }catch (const xml_schema::exception& e){
- std::ostringstream os;
- os << e;
- throw CDynamixelMotorException(_HERE_,os.str());
- }*/
+ /*xml_schema::namespace_infomap map;
+ actualice_parameters();
+
+ unsigned char c[] = {this->compliance.pan.cw_compliance_margin, this->compliance.tilt.cw_compliance_margin};
+ std::vector<unsigned char> cw_compliance_margin(c, c + sizeof(c) / sizeof(unsigned char));
+ c[0] = this->compliance.pan.ccw_compliance_margin;
+ c[1] = this->compliance.tilt.ccw_compliance_margin;
+ std::vector<unsigned char> ccw_compliance_margin(c, c + sizeof(c) / sizeof(unsigned char));
+ c[0] = this->compliance.pan.cw_compliance_slope;
+ c[1] = this->compliance.tilt.cw_compliance_slope;
+ std::vector<unsigned char> cw_compliance_slope(c, c + sizeof(c) / sizeof(unsigned char));
+ c[0] = this->compliance.pan.ccw_compliance_slope;
+ c[1] = this->compliance.tilt.ccw_compliance_slope;
+ std::vector<unsigned char> ccw_compliance_slope(c, c + sizeof(c) / sizeof(unsigned char));
+ unsigned short int i[] ={this->config.pan.punch, this->config.tilt.punch};
+ std::vector<unsigned short int> punch(i, i + sizeof(i) / sizeof(unsigned short int));
+ c[0] = this->pid.pan.p;
+ c[1] = this->pid.tilt.p;
+ std::vector<unsigned char> kp(c, c + sizeof(c) / sizeof(unsigned char));
+ c[0] = this->pid.pan.i;
+ c[1] = this->pid.tilt.i;
+ std::vector<unsigned char> ki(c, c + sizeof(c) / sizeof(unsigned char));
+ c[0] = this->pid.pan.d;
+ c[1] = this->pid.tilt.d;
+ std::vector<unsigned char> kd(c, c + sizeof(c) / sizeof(unsigned char));
+ double d[] = {this->min_angle.pan, this->min_angle.tilt};
+ std::vector<double> min_angles(d, d + sizeof(d) / sizeof(double));
+ d[0] = this->max_angle.pan;
+ d[1] = this->max_angle.tilt;
+ std::vector<double> max_angles(d, d + sizeof(d) / sizeof(double));
+ d[0] = this->checking_hysteresis_offset.pan;
+ d[1] = this->checking_hysteresis_offset.tilt;
+ std::vector<double> checking_hyst_offset(d, d + sizeof(d) / sizeof(double));
+ d[0] = this->checking_hysteresis_slope.pan;
+ d[1] = this->checking_hysteresis_slope.tilt;
+ std::vector<double> checking_hyst_slope(d, d + sizeof(d) / sizeof(double));
+
+ try{
+ std::ofstream output_file(filename.c_str(),std::ios_base::out);
+
+ dynamixel_pan_tilt_config_t dyn_cfg((int)this->pan->get_turn_off_alarms(),
+ this->config.pan.max_temperature,
+ this->config.pan.max_voltage,
+ this->config.pan.min_voltage,
+ this->config.pan.max_torque,
+ cw_compliance_margin,
+ ccw_compliance_margin,
+ cw_compliance_slope,
+ ccw_compliance_slope,
+ punch,
+ kp,
+ ki,
+ kd,
+ min_angles,
+ max_angles,
+ checking_hyst_offset,
+ checking_hyst_slope);
+ map[""].name="";
+ map[""].schema="dynamixel_pan_tilt_cfg_file.xsd";
+ dynamixel_pan_tilt_config(output_file,dyn_cfg,map);
+ }catch (const xml_schema::exception& e){
+ std::ostringstream os;
+ os << e;
+ throw CDynamixelMotorException(_HERE_,os.str());
+ }*/
}
#endif
@@ -484,501 +485,501 @@ 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);
+ 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)
{
- if (this->pan!=NULL)
- {
- 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;
- }
-
- if (this->event_server->event_is_set(this->torque_moving))
- this->event_server->reset_event(this->torque_moving);
- //set_checking_hysteresis_pan(0.0);
- }
- catch(CException &e)
- {
- 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;
- }
- }
- else
- {
- speed = 0.0;
- angle = 0.0;
- throw CDynamixel_Pan_TiltException(_HERE_,"The dynamixel pan servo is not properly configured. Not moving");
- }
+ if (this->pan!=NULL)
+ {
+ 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;
+ }
+
+ if (this->event_server->event_is_set(this->torque_moving))
+ this->event_server->reset_event(this->torque_moving);
+ //set_checking_hysteresis_pan(0.0);
+ }
+ catch(CException &e)
+ {
+ 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;
+ }
+ }
+ else
+ {
+ 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)
{
- if (this->tilt!=NULL)
- {
- 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;
- }
-
- if (this->event_server->event_is_set(this->torque_moving))
- this->event_server->reset_event(this->torque_moving);
- //set_checking_hysteresis_tilt(0.0);
- }
- catch(CException &e)
- {
- 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;
- }
- }
- else
- {
- speed = 0.0;
- angle = 0.0;
- throw CDynamixel_Pan_TiltException(_HERE_,"The dynamixel tilt servo is not properly configured. Not moving");
- }
+ if (this->tilt!=NULL)
+ {
+ 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;
+ }
+
+ if (this->event_server->event_is_set(this->torque_moving))
+ this->event_server->reset_event(this->torque_moving);
+ //set_checking_hysteresis_tilt(0.0);
+ }
+ catch(CException &e)
+ {
+ 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;
+ }
+ }
+ else
+ {
+ speed = 0.0;
+ angle = 0.0;
+ throw CDynamixel_Pan_TiltException(_HERE_,"The dynamixel tilt servo is not properly configured. Not moving");
+ }
}
void CDynamixel_Pan_Tilt::move_relative_angle(Dynamixel_pan_tilt_data &angle, Dynamixel_pan_tilt_data &speed)
{
- move_relative_angle_pan(angle.pan, speed.pan);
- move_relative_angle_tilt(angle.tilt, speed.tilt);
+ move_relative_angle_pan(angle.pan, speed.pan);
+ move_relative_angle_tilt(angle.tilt, speed.tilt);
}
void CDynamixel_Pan_Tilt::move_relative_angle_pan(double &angle, double &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");
- }
+ 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)
{
- 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");
- }
+ 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)
{
- move_torque_pan(speed.pan);
- move_torque_tilt(speed.tilt);
+ move_torque_pan(speed.pan);
+ move_torque_tilt(speed.tilt);
}
void CDynamixel_Pan_Tilt::move_torque_pan(double &speed)
{
- 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;
- bool ctrl_mode_changed = (this->mode.pan == angle_ctrl);
-
- if (speed > 0)
- {
- get_checking_hysteresis_pan(hyst_aux);
- if (pos < (this->max_angle.pan - hyst_aux))
- {
- 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);
- }
- }
- else if (speed < 0)
- {
- get_checking_hysteresis_pan(hyst_aux);
- if (pos > (this->min_angle.pan + hyst_aux))
- {
- 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
- {
- 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 = torque_ctrl;
- this->config.pan.min_angle = -this->info.pan.center_angle;
- this->config.pan.max_angle = -this->info.pan.center_angle;
- }
- }
- else
- {
- speed = 0.0;
- throw CDynamixel_Pan_TiltException(_HERE_,"The dynamixel pan servo is not properly configured. Not moving");
- }
+ 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;
+ bool ctrl_mode_changed = (this->mode.pan == angle_ctrl);
+
+ if (speed > 0)
+ {
+ get_checking_hysteresis_pan(hyst_aux);
+ if (pos < (this->max_angle.pan - hyst_aux))
+ {
+ 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);
+ }
+ }
+ else if (speed < 0)
+ {
+ get_checking_hysteresis_pan(hyst_aux);
+ if (pos > (this->min_angle.pan + hyst_aux))
+ {
+ 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
+ {
+ 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 = torque_ctrl;
+ this->config.pan.min_angle = -this->info.pan.center_angle;
+ this->config.pan.max_angle = -this->info.pan.center_angle;
+ }
+ }
+ else
+ {
+ 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)
{
- 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;
- bool ctrl_mode_changed = (this->mode.tilt == angle_ctrl);
-
- if (speed > 0)
- {
- get_checking_hysteresis_tilt(hyst_aux);
- if (pos < (this->max_angle.tilt - hyst_aux))
- {
- 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);
- }
- }
- else if (speed < 0)
- {
- get_checking_hysteresis_tilt(hyst_aux);
- if (pos > (this->min_angle.tilt + hyst_aux))
- {
- 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
- {
- 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 = torque_ctrl;
- this->config.tilt.min_angle = -this->info.tilt.center_angle;
- this->config.tilt.max_angle = -this->info.tilt.center_angle;
- }
- }
- else
- {
- speed = 0.0;
- throw CDynamixel_Pan_TiltException(_HERE_,"The dynamixel tilt servo is not properly configured. Not moving");
- }
+ 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;
+ bool ctrl_mode_changed = (this->mode.tilt == angle_ctrl);
+
+ if (speed > 0)
+ {
+ get_checking_hysteresis_tilt(hyst_aux);
+ if (pos < (this->max_angle.tilt - hyst_aux))
+ {
+ 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);
+ }
+ }
+ else if (speed < 0)
+ {
+ get_checking_hysteresis_tilt(hyst_aux);
+ if (pos > (this->min_angle.tilt + hyst_aux))
+ {
+ 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
+ {
+ 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 = torque_ctrl;
+ this->config.tilt.min_angle = -this->info.tilt.center_angle;
+ this->config.tilt.max_angle = -this->info.tilt.center_angle;
+ }
+ }
+ else
+ {
+ speed = 0.0;
+ throw CDynamixel_Pan_TiltException(_HERE_,"The dynamixel tilt servo is not properly configured. Not moving");
+ }
}
void *CDynamixel_Pan_Tilt::checking_angle_limits(void *arg)
{
- CDynamixel_Pan_Tilt *pan_tilt = (CDynamixel_Pan_Tilt*) arg;
- std::list<std::string> event_list;
- CEventServer *event_server=CEventServer::instance();
-
- event_list.push_back(pan_tilt->exiting);
- event_list.push_back(pan_tilt->torque_moving);
-
- int event_id;
-
- while (true)
- {
- try
- {
- event_id = event_server->wait_first(event_list);
- if (event_id == 0)//== EXITING
- {
- pthread_exit(NULL);
- }
- else if (event_id == 1)// ==TORQUE_MOVING
- {
- bool is_moving;
- Dynamixel_pan_tilt_data pos;
- pan_tilt->is_torque_moving(is_moving);
- Torque_moving_state pan_state;
- pan_tilt->get_moving_state_pan(pan_state);
- Torque_moving_state tilt_state;
- pan_tilt->get_moving_state_tilt(tilt_state);
- double hyst_aux = 0.0;
-
- while (is_moving)
- {
- pos = pan_tilt->get_position();
- pan_tilt->get_checking_hysteresis_pan(hyst_aux);
- if ((pan_state == positive_torque) && (pos.pan >= (pan_tilt->max_angle.pan - hyst_aux)))
- {
- pan_tilt->pan->stop();
- pan_tilt->set_moving_state_pan(stopped);
- //pan_tilt->checking_hysteresis.pan = 0.0;
-
- // std::cout << "pan out!" << std::endl;
- // pan_tilt->print_pan_tilt_data(pos);
- }
- else if ((pan_state == negative_torque) && (pos.pan <= (pan_tilt->min_angle.pan + hyst_aux)))
- {
- pan_tilt->pan->stop();
- pan_tilt->set_moving_state_pan(stopped);
- //pan_tilt->checking_hysteresis.pan = 0.0;
-
- // std::cout << "pan out!" << std::endl;
- // pan_tilt->print_pan_tilt_data(pos);
- }
-
- pan_tilt->get_checking_hysteresis_tilt(hyst_aux);
- if ((tilt_state == positive_torque) && (pos.tilt >= (pan_tilt->max_angle.tilt - hyst_aux)))
- {
- pan_tilt->tilt->stop();
- pan_tilt->set_moving_state_tilt(stopped);
- //pan_tilt->checking_hysteresis.tilt = 0.0;
-
- // std::cout << "tilt out!" << std::endl;
- // pan_tilt->print_pan_tilt_data(pos);
- }
- else if ((tilt_state == negative_torque) && (pos.tilt <= (pan_tilt->min_angle.tilt + hyst_aux)))
- {
- pan_tilt->tilt->stop();
- pan_tilt->set_moving_state_tilt(stopped);
- //pan_tilt->checking_hysteresis.tilt = 0.0;
-
- // std::cout << "tilt out!" << std::endl;
- // pan_tilt->print_pan_tilt_data(pos);
- }
- usleep(CHECKING_TIME_INTERVAL);
- pan_tilt->is_torque_moving(is_moving);
- pan_tilt->get_moving_state_pan(pan_state);
- pan_tilt->get_moving_state_tilt(tilt_state);
- if (event_server->event_is_set(pan_tilt->exiting))
- {
- pthread_exit(NULL);
- }
- }
- }
- }
- catch (CException &e)
- {
- std::cout << "Checking angle limits exception: " << e.what() << std::endl;
- }
- }
+ CDynamixel_Pan_Tilt *pan_tilt = (CDynamixel_Pan_Tilt*) arg;
+ std::list<std::string> event_list;
+ CEventServer *event_server=CEventServer::instance();
+
+ event_list.push_back(pan_tilt->exiting);
+ event_list.push_back(pan_tilt->torque_moving);
+
+ int event_id;
+
+ while (true)
+ {
+ try
+ {
+ event_id = event_server->wait_first(event_list);
+ if (event_id == 0)//== EXITING
+ {
+ pthread_exit(NULL);
+ }
+ else if (event_id == 1)// ==TORQUE_MOVING
+ {
+ bool is_moving;
+ Dynamixel_pan_tilt_data pos;
+ pan_tilt->is_torque_moving(is_moving);
+ Torque_moving_state pan_state;
+ pan_tilt->get_moving_state_pan(pan_state);
+ Torque_moving_state tilt_state;
+ pan_tilt->get_moving_state_tilt(tilt_state);
+ double hyst_aux = 0.0;
+
+ while (is_moving)
+ {
+ pos = pan_tilt->get_position();
+ pan_tilt->get_checking_hysteresis_pan(hyst_aux);
+ if ((pan_state == positive_torque) && (pos.pan >= (pan_tilt->max_angle.pan - hyst_aux)))
+ {
+ pan_tilt->pan->stop();
+ pan_tilt->set_moving_state_pan(stopped);
+ //pan_tilt->checking_hysteresis.pan = 0.0;
+
+ // std::cout << "pan out!" << std::endl;
+ // pan_tilt->print_pan_tilt_data(pos);
+ }
+ else if ((pan_state == negative_torque) && (pos.pan <= (pan_tilt->min_angle.pan + hyst_aux)))
+ {
+ pan_tilt->pan->stop();
+ pan_tilt->set_moving_state_pan(stopped);
+ //pan_tilt->checking_hysteresis.pan = 0.0;
+
+ // std::cout << "pan out!" << std::endl;
+ // pan_tilt->print_pan_tilt_data(pos);
+ }
+
+ pan_tilt->get_checking_hysteresis_tilt(hyst_aux);
+ if ((tilt_state == positive_torque) && (pos.tilt >= (pan_tilt->max_angle.tilt - hyst_aux)))
+ {
+ pan_tilt->tilt->stop();
+ pan_tilt->set_moving_state_tilt(stopped);
+ //pan_tilt->checking_hysteresis.tilt = 0.0;
+
+ // std::cout << "tilt out!" << std::endl;
+ // pan_tilt->print_pan_tilt_data(pos);
+ }
+ else if ((tilt_state == negative_torque) && (pos.tilt <= (pan_tilt->min_angle.tilt + hyst_aux)))
+ {
+ pan_tilt->tilt->stop();
+ pan_tilt->set_moving_state_tilt(stopped);
+ //pan_tilt->checking_hysteresis.tilt = 0.0;
+
+ // std::cout << "tilt out!" << std::endl;
+ // pan_tilt->print_pan_tilt_data(pos);
+ }
+ usleep(CHECKING_TIME_INTERVAL);
+ pan_tilt->is_torque_moving(is_moving);
+ pan_tilt->get_moving_state_pan(pan_state);
+ pan_tilt->get_moving_state_tilt(tilt_state);
+ if (event_server->event_is_set(pan_tilt->exiting))
+ {
+ pthread_exit(NULL);
+ }
+ }
+ }
+ }
+ catch (CException &e)
+ {
+ std::cout << "Checking angle limits exception: " << e.what() << std::endl;
+ }
+ }
}
void CDynamixel_Pan_Tilt::stop(void)
{
- 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);
+ 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);
}
@@ -988,186 +989,186 @@ void CDynamixel_Pan_Tilt::stop(void)
void CDynamixel_Pan_Tilt::actualice_parameters(void)
{
- actualice_info();
- actualice_pid_control();
- actualice_compliance_control();
- actualice_config();
- actualice_control_mode();
- actualice_angle_limits();
+ actualice_info();
+ actualice_pid_control();
+ actualice_compliance_control();
+ actualice_config();
+ actualice_control_mode();
+ actualice_angle_limits();
}
void CDynamixel_Pan_Tilt::actualice_config(void)
{
- 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");
+ 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)
{
- this->min_angle.pan = this->config.pan.min_angle;
- this->max_angle.pan = this->config.pan.max_angle;
- this->min_angle.tilt = this->config.tilt.min_angle;
- this->max_angle.tilt = this->config.tilt.max_angle;
+ this->min_angle.pan = this->config.pan.min_angle;
+ this->max_angle.pan = this->config.pan.max_angle;
+ this->min_angle.tilt = this->config.tilt.min_angle;
+ this->max_angle.tilt = this->config.tilt.max_angle;
}
void CDynamixel_Pan_Tilt::actualice_control_mode(void)
{
- if (this->pan!=NULL && this->tilt!=NULL)
- {
- 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;
- }
- }
- else
- throw CDynamixel_Pan_TiltException(_HERE_,"The dynamixel pan&tilt is not properly configured. Config not actualiced");
+ if (this->pan!=NULL && this->tilt!=NULL)
+ {
+ 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;
+ }
+ }
+ 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)
{
- if (this->pan!=NULL && this->tilt!=NULL)
- {
- 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;
- }
- }
- else
- throw CDynamixel_Pan_TiltException(_HERE_,"The dynamixel pan&tilt is not properly configured. PID control not actualiced");
+ if (this->pan!=NULL && this->tilt!=NULL)
+ {
+ 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;
+ }
+ }
+ 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)
{
- if (this->pan!=NULL && this->tilt!=NULL)
- {
- 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;
- }
- }
- else
- throw CDynamixel_Pan_TiltException(_HERE_,"The dynamixel pan&tilt is not properly configured. Info not actualiced");
+ if (this->pan!=NULL && this->tilt!=NULL)
+ {
+ 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;
+ }
+ }
+ 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)
{
- if (this->pan!=NULL && this->tilt!=NULL)
- {
- 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;
- }
- }
- else
- throw CDynamixel_Pan_TiltException(_HERE_,"The dynamixel pan&tilt is not properly configured. Compliance control not actualiced");
+ if (this->pan!=NULL && this->tilt!=NULL)
+ {
+ 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;
+ }
+ }
+ 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)
{
- this->info.pan.model="";
- this->info.pan.firmware_ver=(unsigned char)-1;
- this->info.pan.gear_ratio=(unsigned int)-1;
- this->info.pan.encoder_resolution=(unsigned int)-1;
- this->info.pan.pid_control=false;
- this->info.pan.max_angle=-1;
- this->info.pan.center_angle=-1;
- this->info.pan.max_speed=-1;
- this->info.pan.baudrate=(unsigned int)-1;
- this->info.pan.id=(unsigned char)-1;
- this->compliance.pan.cw_compliance_margin=0x00;
- this->compliance.pan.ccw_compliance_margin=0x00;
- this->compliance.pan.cw_compliance_slope=0x20;
- this->compliance.pan.ccw_compliance_slope=0x20;
- this->pid.pan.p=0x00;
- this->pid.pan.i=0x00;
- this->pid.pan.d=0x00;
- this->config.pan.max_angle=0.0;
- this->config.pan.min_angle=0.0;
- this->config.pan.max_temperature=0.0;
- this->config.pan.max_voltage=0.0;
- this->config.pan.min_voltage=0.0;
- this->config.pan.max_torque=0.0;
- this->min_angle.pan=0.0;
- this->max_angle.pan=0.0;
-
- this->info.tilt.model="";
- this->info.tilt.firmware_ver=(unsigned char)-1;
- this->info.tilt.gear_ratio=(unsigned int)-1;
- this->info.tilt.encoder_resolution=(unsigned int)-1;
- this->info.tilt.pid_control=false;
- this->info.tilt.max_angle=-1;
- this->info.tilt.center_angle=-1;
- this->info.tilt.max_speed=-1;
- this->info.tilt.baudrate=(unsigned int)-1;
- this->info.tilt.id=(unsigned char)-1;
- this->compliance.tilt.cw_compliance_margin=0x00;
- this->compliance.tilt.ccw_compliance_margin=0x00;
- this->compliance.tilt.cw_compliance_slope=0x20;
- this->compliance.tilt.ccw_compliance_slope=0x20;
- this->pid.tilt.p=0x00;
- this->pid.tilt.i=0x00;
- this->pid.tilt.d=0x00;
- this->config.tilt.max_angle=0.0;
- this->config.tilt.min_angle=0.0;
- this->config.tilt.max_temperature=0.0;
- this->config.tilt.max_voltage=0.0;
- this->config.tilt.min_voltage=0.0;
- this->config.tilt.max_torque=0.0;
- this->min_angle.tilt=0.0;
- this->max_angle.tilt=0.0;
- 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;
- this->checking_hysteresis_offset.tilt = 0.0;
+ this->info.pan.model="";
+ this->info.pan.firmware_ver=(unsigned char)-1;
+ this->info.pan.gear_ratio=(unsigned int)-1;
+ this->info.pan.encoder_resolution=(unsigned int)-1;
+ this->info.pan.pid_control=false;
+ this->info.pan.max_angle=-1;
+ this->info.pan.center_angle=-1;
+ this->info.pan.max_speed=-1;
+ this->info.pan.baudrate=(unsigned int)-1;
+ this->info.pan.id=(unsigned char)-1;
+ this->compliance.pan.cw_compliance_margin=0x00;
+ this->compliance.pan.ccw_compliance_margin=0x00;
+ this->compliance.pan.cw_compliance_slope=0x20;
+ this->compliance.pan.ccw_compliance_slope=0x20;
+ this->pid.pan.p=0x00;
+ this->pid.pan.i=0x00;
+ this->pid.pan.d=0x00;
+ this->config.pan.max_angle=0.0;
+ this->config.pan.min_angle=0.0;
+ this->config.pan.max_temperature=0.0;
+ this->config.pan.max_voltage=0.0;
+ this->config.pan.min_voltage=0.0;
+ this->config.pan.max_torque=0.0;
+ this->min_angle.pan=0.0;
+ this->max_angle.pan=0.0;
+
+ this->info.tilt.model="";
+ this->info.tilt.firmware_ver=(unsigned char)-1;
+ this->info.tilt.gear_ratio=(unsigned int)-1;
+ this->info.tilt.encoder_resolution=(unsigned int)-1;
+ this->info.tilt.pid_control=false;
+ this->info.tilt.max_angle=-1;
+ this->info.tilt.center_angle=-1;
+ this->info.tilt.max_speed=-1;
+ this->info.tilt.baudrate=(unsigned int)-1;
+ this->info.tilt.id=(unsigned char)-1;
+ this->compliance.tilt.cw_compliance_margin=0x00;
+ this->compliance.tilt.ccw_compliance_margin=0x00;
+ this->compliance.tilt.cw_compliance_slope=0x20;
+ this->compliance.tilt.ccw_compliance_slope=0x20;
+ this->pid.tilt.p=0x00;
+ this->pid.tilt.i=0x00;
+ this->pid.tilt.d=0x00;
+ this->config.tilt.max_angle=0.0;
+ this->config.tilt.min_angle=0.0;
+ this->config.tilt.max_temperature=0.0;
+ this->config.tilt.max_voltage=0.0;
+ this->config.tilt.min_voltage=0.0;
+ this->config.tilt.max_torque=0.0;
+ this->min_angle.tilt=0.0;
+ this->max_angle.tilt=0.0;
+ 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;
+ this->checking_hysteresis_offset.tilt = 0.0;
}
@@ -1177,367 +1178,367 @@ void CDynamixel_Pan_Tilt::default_parameters(void)
Dynamixel_pan_tilt_data CDynamixel_Pan_Tilt::get_position(void)
{
- Dynamixel_pan_tilt_data data;
+ Dynamixel_pan_tilt_data data;
- data.pan = get_pan_position();
- data.tilt = get_tilt_position();
+ data.pan = get_pan_position();
+ data.tilt = get_tilt_position();
- return data;
+ return data;
}
double CDynamixel_Pan_Tilt::get_pan_position(void)
{
- if (this->pan!=NULL)
- {
- 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");
+ if (this->pan!=NULL)
+ {
+ 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)
{
- if (this->tilt!=NULL)
- {
- 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");
+ if (this->tilt!=NULL)
+ {
+ 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)
{
- compliance = this->compliance;
+ compliance = this->compliance;
}
void CDynamixel_Pan_Tilt::set_compliance_control(Dynamixel_pan_tilt_compliance &compliance)
{
- if (this->pan!=NULL && this->tilt!=NULL)
- {
- 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
- {
- compliance = this->compliance;
- throw CDynamixel_Pan_TiltException(_HERE_,"The dynamixel pan&tilt is not properly configured. Compliance control not set");
- }
+ if (this->pan!=NULL && this->tilt!=NULL)
+ {
+ 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
+ {
+ 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)
{
- pid = this->pid;
+ pid = this->pid;
}
void CDynamixel_Pan_Tilt::set_pid_control(Dynamixel_pan_tilt_pid &pid)
{
- if (this->pan!=NULL && this->tilt!=NULL)
- {
- 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
- {
- pid = this->pid;
- throw CDynamixel_Pan_TiltException(_HERE_,"The dynamixel pan&tilt is not properly configured. PID control not set");
- }
+ if (this->pan!=NULL && this->tilt!=NULL)
+ {
+ 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
+ {
+ pid = this->pid;
+ throw CDynamixel_Pan_TiltException(_HERE_,"The dynamixel pan&tilt is not properly configured. PID control not set");
+ }
}
void CDynamixel_Pan_Tilt::get_pan_tilt_config(Dynamixel_pan_tilt_config &config)
{
- config.pan.max_angle = this->config.pan.max_angle;
- config.pan.min_angle = this->config.pan.min_angle;
- config.pan.max_temperature = this->config.pan.max_temperature;
- config.pan.max_voltage = this->config.pan.max_voltage;
- config.pan.min_voltage = this->config.pan.min_voltage;
- config.pan.max_torque = this->config.pan.max_torque;
- config.pan.punch = this->config.pan.punch;
-
- config.tilt.max_angle = this->config.tilt.max_angle;
- config.tilt.min_angle = this->config.tilt.min_angle;
- config.tilt.max_temperature = this->config.tilt.max_temperature;
- config.tilt.max_voltage = this->config.tilt.max_voltage;
- config.tilt.min_voltage = this->config.tilt.min_voltage;
- config.tilt.max_torque = this->config.tilt.max_torque;
- config.tilt.punch = this->config.tilt.punch;
+ config.pan.max_angle = this->config.pan.max_angle;
+ config.pan.min_angle = this->config.pan.min_angle;
+ config.pan.max_temperature = this->config.pan.max_temperature;
+ config.pan.max_voltage = this->config.pan.max_voltage;
+ config.pan.min_voltage = this->config.pan.min_voltage;
+ config.pan.max_torque = this->config.pan.max_torque;
+ config.pan.punch = this->config.pan.punch;
+
+ config.tilt.max_angle = this->config.tilt.max_angle;
+ config.tilt.min_angle = this->config.tilt.min_angle;
+ config.tilt.max_temperature = this->config.tilt.max_temperature;
+ config.tilt.max_voltage = this->config.tilt.max_voltage;
+ config.tilt.min_voltage = this->config.tilt.min_voltage;
+ config.tilt.max_torque = this->config.tilt.max_torque;
+ config.tilt.punch = this->config.tilt.punch;
}
void CDynamixel_Pan_Tilt::get_pan_tilt_info(Dynamixel_pan_tilt_info &info)
{
- info.pan.model = this->info.pan.model;
- info.pan.firmware_ver = this->info.pan.firmware_ver;
- info.pan.gear_ratio = this->info.pan.gear_ratio;
- info.pan.encoder_resolution = this->info.pan.encoder_resolution;
- info.pan.pid_control = this->info.pan.pid_control;
- info.pan.max_angle = this->info.pan.max_angle;
- info.pan.center_angle = this->info.pan.center_angle;
- info.pan.max_speed = this->info.pan.max_speed;
- info.pan.baudrate = this->info.pan.baudrate;
- info.pan.id = this->info.pan.id;
-
- info.tilt.model = this->info.tilt.model;
- info.tilt.firmware_ver = this->info.tilt.firmware_ver;
- info.tilt.gear_ratio = this->info.tilt.gear_ratio;
- info.tilt.encoder_resolution = this->info.tilt.encoder_resolution;
- info.tilt.pid_control = this->info.tilt.pid_control;
- info.tilt.max_angle = this->info.tilt.max_angle;
- info.tilt.center_angle = this->info.tilt.center_angle;
- info.tilt.max_speed = this->info.tilt.max_speed;
- info.tilt.baudrate = this->info.tilt.baudrate;
- info.tilt.id = this->info.tilt.id;
+ info.pan.model = this->info.pan.model;
+ info.pan.firmware_ver = this->info.pan.firmware_ver;
+ info.pan.gear_ratio = this->info.pan.gear_ratio;
+ info.pan.encoder_resolution = this->info.pan.encoder_resolution;
+ info.pan.pid_control = this->info.pan.pid_control;
+ info.pan.max_angle = this->info.pan.max_angle;
+ info.pan.center_angle = this->info.pan.center_angle;
+ info.pan.max_speed = this->info.pan.max_speed;
+ info.pan.baudrate = this->info.pan.baudrate;
+ info.pan.id = this->info.pan.id;
+
+ info.tilt.model = this->info.tilt.model;
+ info.tilt.firmware_ver = this->info.tilt.firmware_ver;
+ info.tilt.gear_ratio = this->info.tilt.gear_ratio;
+ info.tilt.encoder_resolution = this->info.tilt.encoder_resolution;
+ info.tilt.pid_control = this->info.tilt.pid_control;
+ info.tilt.max_angle = this->info.tilt.max_angle;
+ info.tilt.center_angle = this->info.tilt.center_angle;
+ info.tilt.max_speed = this->info.tilt.max_speed;
+ info.tilt.baudrate = this->info.tilt.baudrate;
+ info.tilt.id = this->info.tilt.id;
}
void CDynamixel_Pan_Tilt::reset_torque_moving_state(void)
{
- pthread_mutex_lock(&(this->mut));
- this->moving_state.pan = stopped;
- this->moving_state.tilt = stopped;
- pthread_mutex_unlock(&(this->mut));
+ pthread_mutex_lock(&(this->mut));
+ this->moving_state.pan = stopped;
+ this->moving_state.tilt = stopped;
+ pthread_mutex_unlock(&(this->mut));
}
void CDynamixel_Pan_Tilt::is_torque_moving(bool &b)
{
- pthread_mutex_lock(&(this->mut));
- b = (this->moving_state.pan != stopped || this->moving_state.tilt != stopped);
- pthread_mutex_unlock(&(this->mut));
+ pthread_mutex_lock(&(this->mut));
+ b = (this->moving_state.pan != stopped || this->moving_state.tilt != stopped);
+ pthread_mutex_unlock(&(this->mut));
}
void CDynamixel_Pan_Tilt::set_moving_state_pan(Torque_moving_state tms)
{
- pthread_mutex_lock(&(this->mut));
- this->moving_state.pan = tms;
- pthread_mutex_unlock(&(this->mut));
+ pthread_mutex_lock(&(this->mut));
+ this->moving_state.pan = tms;
+ pthread_mutex_unlock(&(this->mut));
}
void CDynamixel_Pan_Tilt::set_moving_state_tilt(Torque_moving_state tms)
{
- pthread_mutex_lock(&(this->mut));
- this->moving_state.tilt = tms;
- pthread_mutex_unlock(&(this->mut));
+ pthread_mutex_lock(&(this->mut));
+ this->moving_state.tilt = tms;
+ pthread_mutex_unlock(&(this->mut));
}
void CDynamixel_Pan_Tilt::get_moving_state_pan(Torque_moving_state &tms)
{
- pthread_mutex_lock(&(this->mut));
- tms = this->moving_state.pan;
- pthread_mutex_unlock(&(this->mut));
+ pthread_mutex_lock(&(this->mut));
+ tms = this->moving_state.pan;
+ pthread_mutex_unlock(&(this->mut));
}
void CDynamixel_Pan_Tilt::get_moving_state_tilt(Torque_moving_state &tms)
{
- pthread_mutex_lock(&(this->mut));
- tms = this->moving_state.tilt;
- pthread_mutex_unlock(&(this->mut));
+ pthread_mutex_lock(&(this->mut));
+ tms = this->moving_state.tilt;
+ pthread_mutex_unlock(&(this->mut));
}
void CDynamixel_Pan_Tilt::get_checking_hysteresis_pan(double &hys)
{
- pthread_mutex_lock(&(this->mut));
- hys = this->checking_hysteresis.pan;
- pthread_mutex_unlock(&(this->mut));
+ pthread_mutex_lock(&(this->mut));
+ hys = this->checking_hysteresis.pan;
+ pthread_mutex_unlock(&(this->mut));
}
void CDynamixel_Pan_Tilt::get_checking_hysteresis_tilt(double &hys)
{
- pthread_mutex_lock(&(this->mut));
- hys = this->checking_hysteresis.tilt;
- pthread_mutex_unlock(&(this->mut));
+ pthread_mutex_lock(&(this->mut));
+ hys = this->checking_hysteresis.tilt;
+ pthread_mutex_unlock(&(this->mut));
}
void CDynamixel_Pan_Tilt::set_checking_hysteresis_pan(double hys)
{
- pthread_mutex_lock(&(this->mut));
- this->checking_hysteresis.pan = hys;
- pthread_mutex_unlock(&(this->mut));
+ pthread_mutex_lock(&(this->mut));
+ this->checking_hysteresis.pan = hys;
+ pthread_mutex_unlock(&(this->mut));
}
void CDynamixel_Pan_Tilt::set_checking_hysteresis_tilt(double hys)
{
- pthread_mutex_lock(&(this->mut));
- this->checking_hysteresis.tilt = hys;
- pthread_mutex_unlock(&(this->mut));
+ pthread_mutex_lock(&(this->mut));
+ this->checking_hysteresis.tilt = hys;
+ pthread_mutex_unlock(&(this->mut));
}
double CDynamixel_Pan_Tilt::calculate_checking_hysteresis_pan(double effort)
{
- if (effort == 0.0)
- {
- return 0.0;
- }
- else
- {
- return this->checking_hysteresis_slope.pan*effort - checking_hysteresis_offset.pan;
- }
+ if (effort == 0.0)
+ {
+ return 0.0;
+ }
+ else
+ {
+ return this->checking_hysteresis_slope.pan*effort - checking_hysteresis_offset.pan;
+ }
}
double CDynamixel_Pan_Tilt::calculate_checking_hysteresis_tilt(double effort)
{
- if (effort == 0.0)
- {
- return 0.0;
- }
- else
- {
- return this->checking_hysteresis_slope.tilt*effort - checking_hysteresis_offset.tilt;
- }
+ if (effort == 0.0)
+ {
+ return 0.0;
+ }
+ else
+ {
+ return this->checking_hysteresis_slope.tilt*effort - checking_hysteresis_offset.tilt;
+ }
}
Dynamixel_pan_tilt_data CDynamixel_Pan_Tilt::get_current_speed(void)
{
- 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");
+ 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)
{
- 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");
+ 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");
- }
+ 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");
+ 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");
}
Dynamixel_pan_tilt_data CDynamixel_Pan_Tilt::get_max_angle_limit(void)
{
- if (this->pan != NULL && this->tilt != NULL)
- {
- pthread_mutex_lock(&(this->mut));
- Dynamixel_pan_tilt_data data;
- data.pan = this->max_angle.pan;
- data.tilt = this->max_angle.tilt;
- pthread_mutex_unlock(&(this->mut));
- return data;
- }
- else
- throw CDynamixel_Pan_TiltException(_HERE_, "The dynamixel pan&tilt is not properly configured. Max angle not get.");
+ if (this->pan != NULL && this->tilt != NULL)
+ {
+ pthread_mutex_lock(&(this->mut));
+ Dynamixel_pan_tilt_data data;
+ data.pan = this->max_angle.pan;
+ data.tilt = this->max_angle.tilt;
+ pthread_mutex_unlock(&(this->mut));
+ return data;
+ }
+ else
+ throw CDynamixel_Pan_TiltException(_HERE_, "The dynamixel pan&tilt is not properly configured. Max angle not get.");
}
Dynamixel_pan_tilt_data CDynamixel_Pan_Tilt::get_min_angle_limit(void)
{
- if (this->pan != NULL && this->tilt != NULL)
- {
- pthread_mutex_lock(&(this->mut));
- Dynamixel_pan_tilt_data data;
- data.pan = this->min_angle.pan;
- data.tilt = this->min_angle.tilt;
- pthread_mutex_unlock(&(this->mut));
- return data;
- }
- else
- throw CDynamixel_Pan_TiltException(_HERE_, "The dynamixel pan&tilt is not properly configured. Min angle not get.");
+ if (this->pan != NULL && this->tilt != NULL)
+ {
+ pthread_mutex_lock(&(this->mut));
+ Dynamixel_pan_tilt_data data;
+ data.pan = this->min_angle.pan;
+ data.tilt = this->min_angle.tilt;
+ pthread_mutex_unlock(&(this->mut));
+ return data;
+ }
+ else
+ throw CDynamixel_Pan_TiltException(_HERE_, "The dynamixel pan&tilt is not properly configured. Min angle not get.");
}
///------------------------------------------------------Get & Set
@@ -1546,30 +1547,30 @@ Dynamixel_pan_tilt_data CDynamixel_Pan_Tilt::get_min_angle_limit(void)
std::ostream& operator<< (std::ostream& out, Dynamixel_pan_tilt_data &data)
{
- out << "(" << data.pan << " , " << data.tilt << ")" << std::endl;
- return out;
+ out << "(" << data.pan << " , " << data.tilt << ")" << std::endl;
+ return out;
}
void CDynamixel_Pan_Tilt::print_current_position(void)
{
- 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.");
+ 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)
{
- std::cout << "(" << data.pan << " , " << data.tilt << ")" << std::endl;
+ std::cout << "(" << data.pan << " , " << data.tilt << ")" << std::endl;
}
///--------------------------------------------------------Prints
diff --git a/src/dynamixel_pan_tilt.h b/src/dynamixel_pan_tilt.h
index d37a413..ddf5917 100644
--- a/src/dynamixel_pan_tilt.h
+++ b/src/dynamixel_pan_tilt.h
@@ -45,8 +45,8 @@
*/
typedef struct
{
- double pan;
- double tilt;
+ double pan;
+ double tilt;
}Dynamixel_pan_tilt_data;
/**
@@ -70,8 +70,8 @@ typedef struct
typedef struct
{
- TDynamixel_config pan;
- TDynamixel_config tilt;
+ TDynamixel_config pan;
+ TDynamixel_config tilt;
}Dynamixel_pan_tilt_config;
/**
@@ -98,8 +98,8 @@ typedef struct
typedef struct
{
- TDynamixel_info pan;
- TDynamixel_info tilt;
+ TDynamixel_info pan;
+ TDynamixel_info tilt;
}Dynamixel_pan_tilt_info;
/**
@@ -120,8 +120,8 @@ typedef struct
typedef struct
{
- TDynamixel_compliance pan;
- TDynamixel_compliance tilt;
+ TDynamixel_compliance pan;
+ TDynamixel_compliance tilt;
}Dynamixel_pan_tilt_compliance;
/**
@@ -141,8 +141,8 @@ typedef struct
typedef struct
{
- TDynamixel_pid pan;
- TDynamixel_pid tilt;
+ TDynamixel_pid pan;
+ TDynamixel_pid tilt;
}Dynamixel_pan_tilt_pid;
/**
@@ -161,8 +161,8 @@ typedef struct
typedef struct
{
- control_mode pan;
- control_mode tilt;
+ control_mode pan;
+ control_mode tilt;
}Control_mode_pan_tilt;
typedef enum {positive_torque = 1, stopped = 0, negative_torque = -1} Torque_moving_state;
@@ -184,8 +184,8 @@ typedef enum {positive_torque = 1, stopped = 0, negative_torque = -1} Torque_mov
typedef struct
{
- Torque_moving_state pan;
- Torque_moving_state tilt;
+ Torque_moving_state pan;
+ Torque_moving_state tilt;
}Torque_pan_tilt_moving_state;
/**
@@ -201,9 +201,9 @@ typedef struct
typedef struct
{
- Dynamixel_pan_tilt_data max_torque;
- Dynamixel_pan_tilt_data max_angle;
- Dynamixel_pan_tilt_data min_angle;
+ Dynamixel_pan_tilt_data max_torque;
+ Dynamixel_pan_tilt_data max_angle;
+ Dynamixel_pan_tilt_data min_angle;
}Dynamixel_pan_tilt_xml_limits;
@@ -248,431 +248,431 @@ typedef struct
class CDynamixel_Pan_Tilt
{
- private:
- CDynamixelMotor *pan;
- CDynamixelMotor *tilt;
-
- Dynamixel_pan_tilt_data min_angle, max_angle;
-
- Dynamixel_pan_tilt_config config;
- Dynamixel_pan_tilt_info info;
- Dynamixel_pan_tilt_compliance compliance;
- Dynamixel_pan_tilt_pid pid;
- Control_mode_pan_tilt mode;
-
- pthread_t checking_thread;
- pthread_mutex_t mut;
- Torque_pan_tilt_moving_state moving_state;
- Dynamixel_pan_tilt_data checking_hysteresis, checking_hysteresis_offset, checking_hysteresis_slope;
-
- CEventServer *event_server;
- std::string exiting;
- std::string torque_moving;
-
-
- protected:
- /**
- * \brief Function to set all the parameters to the default value.
- *
- */
- void default_parameters(void);
-
- /**
- * \brief Function to actualice all the variables with all the parameters of both servos.
- *
- */
- void actualice_parameters(void);
-
- /**
- * \brief Function to actualice the variable of configuration of both servos.
- *
- */
- void actualice_config(void);
-
- /**
- * \brief Function to actualice the variable of compliance control of the pan-tilt.
- *
- */
- void actualice_compliance_control(void);
-
- /**
- * \brief Function to actualice the variable of the angle limits of the pan-tilt.
- *
- */
- void actualice_angle_limits(void);
-
- /**
- * \brief Function to actualice the variable of control mode of the pan-tilt.
- *
- */
- void actualice_control_mode(void);
-
- /**
- * \brief Function to actualice the variable of pid control of the pan-tilt.
- *
- */
- void actualice_pid_control(void);
-
- /**
- * \brief Function to actualice the variable with the information of the pan-tilt.
- *
- */
- void actualice_info(void);
-
- /**
- * \brief Function to be called for the thread in charge to check the angle limits on torque movement.
- *
- * \param arg A pointer to the class.
- */
- static void *checking_angle_limits(void *arg);
-
- /**
- * \brief Function to set the both servos_state to stopped.
- *
- */
- void reset_torque_moving_state(void);
-
- /**
- * \brief Function that returns if any servo is moving on torque mode.
- *
- * \param b The variable where is going to be returned the value.
- */
- void is_torque_moving(bool &b);
-
- /**
- * \brief Function to set a value on pan's moving state.
- *
- * \param tms The value desired.
- */
- void set_moving_state_pan(Torque_moving_state tms);
-
- /**
- * \brief Function to get the value of pan's moving state.
- *
- * \param tms The variable to save the value of the pan's moving state.
- */
- void get_moving_state_pan(Torque_moving_state &tms);
-
- /**
- * \brief Function to set a value on tilt's moving state.
- *
- * \param tms The value desired.
- */
- void set_moving_state_tilt(Torque_moving_state tms);
-
- /**
- * \brief Function to get the value of tilt's moving state.
- *
- * \param tms The variable to save the value of the tilt's moving state.
- */
- void get_moving_state_tilt(Torque_moving_state &tms);
-
- /**
- * \brief Function to calculate the angle hysteresis depending on the desired effort for the pan servo.
- *
- * \param effort The desired effort for the pan servo.
- *
- * \return The hysteresis angle for the pan servo.
- */
- double calculate_checking_hysteresis_pan(double effort);
-
- /**
- * \brief Function to calculate the angle hysteresis depending on the desired effort for the tilt servo.
- *
- * \param effort The desired effort for the tilt servo.
- *
- * \return The hysteresis angle for the tilt servo.
- */
- double calculate_checking_hysteresis_tilt(double effort);
-
- /**
- * \brief Function to get the value of pan's hysteresis.
- *
- * \param hyst The variable to save the value.
- */
- void get_checking_hysteresis_pan(double &hyst);
-
- /**
- * \brief Function to get the value of tilt's hysteresis.
- *
- * \param hyst The variable to save the value.
- */
- void get_checking_hysteresis_tilt(double &hyst);
-
- /**
- * \brief Function to set the value of pan's hysteresis.
- *
- * \param hyst The value to set.
- */
- void set_checking_hysteresis_pan(double hyst);
-
- /**
- * \brief Function to set the value of tilt's hysteresis.
- *
- * \param hyst The value to set.
- */
- void set_checking_hysteresis_tilt(double hyst);
-
- public:
-
- /**
- * \brief The class constructor.
- *
- * \param name_pan_tilt The user friendly name for the pan-tilt.
- * \param dyn_server_pan_tilt The server for the communication.
- * \param dev_id_pan The ID number for the pan servo.
- * \param dev_id_tilt The ID number for the tilt servo.
- */
- 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=dyn_version1);
-
- /**
- * \brief The destructor.
- *
- */
- virtual ~CDynamixel_Pan_Tilt();
-
- /**
- * \brief Function for the configuration of the pan-tilt.
- * If some parameters are changed internally, it's actualiced on the config variable.
- *
- * \param config The configuration structure.
- */
- void load_config(Dynamixel_pan_tilt_config &config);
-
- /**
- * \brief Function to get the information of the pan-tilt.
- *
- * \param info The variable where it'll be saved the information.
- */
- void get_pan_tilt_info(Dynamixel_pan_tilt_info &info);
-
- /**
- * \brief Function to get the configuration of the pan-tilt.
- *
- * \param config The variable where it'll be saved the configuration's information.
- */
- void get_pan_tilt_config(Dynamixel_pan_tilt_config &config);
+ private:
+ CDynamixelMotor *pan;
+ CDynamixelMotor *tilt;
+
+ Dynamixel_pan_tilt_data min_angle, max_angle;
+
+ Dynamixel_pan_tilt_config config;
+ Dynamixel_pan_tilt_info info;
+ Dynamixel_pan_tilt_compliance compliance;
+ Dynamixel_pan_tilt_pid pid;
+ Control_mode_pan_tilt mode;
+
+ pthread_t checking_thread;
+ pthread_mutex_t mut;
+ Torque_pan_tilt_moving_state moving_state;
+ Dynamixel_pan_tilt_data checking_hysteresis, checking_hysteresis_offset, checking_hysteresis_slope;
+
+ CEventServer *event_server;
+ std::string exiting;
+ std::string torque_moving;
+
+
+ protected:
+ /**
+ * \brief Function to set all the parameters to the default value.
+ *
+ */
+ void default_parameters(void);
+
+ /**
+ * \brief Function to actualice all the variables with all the parameters of both servos.
+ *
+ */
+ void actualice_parameters(void);
+
+ /**
+ * \brief Function to actualice the variable of configuration of both servos.
+ *
+ */
+ void actualice_config(void);
+
+ /**
+ * \brief Function to actualice the variable of compliance control of the pan-tilt.
+ *
+ */
+ void actualice_compliance_control(void);
+
+ /**
+ * \brief Function to actualice the variable of the angle limits of the pan-tilt.
+ *
+ */
+ void actualice_angle_limits(void);
+
+ /**
+ * \brief Function to actualice the variable of control mode of the pan-tilt.
+ *
+ */
+ void actualice_control_mode(void);
+
+ /**
+ * \brief Function to actualice the variable of pid control of the pan-tilt.
+ *
+ */
+ void actualice_pid_control(void);
+
+ /**
+ * \brief Function to actualice the variable with the information of the pan-tilt.
+ *
+ */
+ void actualice_info(void);
+
+ /**
+ * \brief Function to be called for the thread in charge to check the angle limits on torque movement.
+ *
+ * \param arg A pointer to the class.
+ */
+ static void *checking_angle_limits(void *arg);
+
+ /**
+ * \brief Function to set the both servos_state to stopped.
+ *
+ */
+ void reset_torque_moving_state(void);
+
+ /**
+ * \brief Function that returns if any servo is moving on torque mode.
+ *
+ * \param b The variable where is going to be returned the value.
+ */
+ void is_torque_moving(bool &b);
+
+ /**
+ * \brief Function to set a value on pan's moving state.
+ *
+ * \param tms The value desired.
+ */
+ void set_moving_state_pan(Torque_moving_state tms);
+
+ /**
+ * \brief Function to get the value of pan's moving state.
+ *
+ * \param tms The variable to save the value of the pan's moving state.
+ */
+ void get_moving_state_pan(Torque_moving_state &tms);
+
+ /**
+ * \brief Function to set a value on tilt's moving state.
+ *
+ * \param tms The value desired.
+ */
+ void set_moving_state_tilt(Torque_moving_state tms);
+
+ /**
+ * \brief Function to get the value of tilt's moving state.
+ *
+ * \param tms The variable to save the value of the tilt's moving state.
+ */
+ void get_moving_state_tilt(Torque_moving_state &tms);
+
+ /**
+ * \brief Function to calculate the angle hysteresis depending on the desired effort for the pan servo.
+ *
+ * \param effort The desired effort for the pan servo.
+ *
+ * \return The hysteresis angle for the pan servo.
+ */
+ double calculate_checking_hysteresis_pan(double effort);
+
+ /**
+ * \brief Function to calculate the angle hysteresis depending on the desired effort for the tilt servo.
+ *
+ * \param effort The desired effort for the tilt servo.
+ *
+ * \return The hysteresis angle for the tilt servo.
+ */
+ double calculate_checking_hysteresis_tilt(double effort);
+
+ /**
+ * \brief Function to get the value of pan's hysteresis.
+ *
+ * \param hyst The variable to save the value.
+ */
+ void get_checking_hysteresis_pan(double &hyst);
+
+ /**
+ * \brief Function to get the value of tilt's hysteresis.
+ *
+ * \param hyst The variable to save the value.
+ */
+ void get_checking_hysteresis_tilt(double &hyst);
+
+ /**
+ * \brief Function to set the value of pan's hysteresis.
+ *
+ * \param hyst The value to set.
+ */
+ void set_checking_hysteresis_pan(double hyst);
+
+ /**
+ * \brief Function to set the value of tilt's hysteresis.
+ *
+ * \param hyst The value to set.
+ */
+ void set_checking_hysteresis_tilt(double hyst);
+
+ public:
+
+ /**
+ * \brief The class constructor.
+ *
+ * \param name_pan_tilt The user friendly name for the pan-tilt.
+ * \param dyn_server_pan_tilt The server for the communication.
+ * \param dev_id_pan The ID number for the pan servo.
+ * \param dev_id_tilt The ID number for the tilt servo.
+ */
+ 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=dyn_version1);
+
+ /**
+ * \brief The destructor.
+ *
+ */
+ virtual ~CDynamixel_Pan_Tilt();
+
+ /**
+ * \brief Function for the configuration of the pan-tilt.
+ * If some parameters are changed internally, it's actualiced on the config variable.
+ *
+ * \param config The configuration structure.
+ */
+ void load_config(Dynamixel_pan_tilt_config &config);
+
+ /**
+ * \brief Function to get the information of the pan-tilt.
+ *
+ * \param info The variable where it'll be saved the information.
+ */
+ void get_pan_tilt_info(Dynamixel_pan_tilt_info &info);
+
+ /**
+ * \brief Function to get the configuration of the pan-tilt.
+ *
+ * \param config The variable where it'll be saved the configuration's information.
+ */
+ void get_pan_tilt_config(Dynamixel_pan_tilt_config &config);
#ifdef _HAVE_XSD
-
- /**
- * \brief Function to set the configuration of the pan-tilt reading the information from a specific file.
- *
- * \param filename The file where it's going to be read the pan configuration.
- */
- void load_config(std::string &filename);
-
- /**
- * \brief Static function to read the configuration of the pan-tilt reading the information from a specific file.
- *
- * \param filename The file where it's going to be read the pan configuration.
- * \param xml_limits The struct where is going to be saved the information.
- */
- static void read_config(std::string &filename, Dynamixel_pan_tilt_xml_limits &xml_limits);
-
- /**
- * \brief Function to save the current configuration of the pan-tilt on a specific file.
- *
- * \param filename The file where it's going to be saved the pan configuration.
- */
- void save_config(std::string &filename);
+
+ /**
+ * \brief Function to set the configuration of the pan-tilt reading the information from a specific file.
+ *
+ * \param filename The file where it's going to be read the pan configuration.
+ */
+ void load_config(std::string &filename);
+
+ /**
+ * \brief Static function to read the configuration of the pan-tilt reading the information from a specific file.
+ *
+ * \param filename The file where it's going to be read the pan configuration.
+ * \param xml_limits The struct where is going to be saved the information.
+ */
+ static void read_config(std::string &filename, Dynamixel_pan_tilt_xml_limits &xml_limits);
+
+ /**
+ * \brief Function to save the current configuration of the pan-tilt on a specific file.
+ *
+ * \param filename The file where it's going to be saved the pan configuration.
+ */
+ void save_config(std::string &filename);
#endif
-
- /**
- * \brief Function to get the position of both servos in degrees.
- *
- * \return A struct with the angles of both servos in degrees.
- */
- Dynamixel_pan_tilt_data get_position(void);
-
- /**
- * \brief Function to get the position of the pan servo in degrees.
- *
- * \return The angle of the pan servo in degrees.
- */
- double get_pan_position(void);
-
- /**
- * \brief Function to get the position of the tilt servo in degrees.
- *
- * \return The angle of the tilt servo in degrees.
- */
- double get_tilt_position(void);
-
- /**
- * \brief Function to move the pan-tilt to a specific position by a specific speed.
- * If some of the values passed are out of range, they'll be changed to the value of the limit.
- *
- * \param angle The specific position in degrees for both servos in a struct.
- * \param speed The specific speed in degrees/second for both servos in a struct.
- */
- void move_absolute_angle(Dynamixel_pan_tilt_data &angle, Dynamixel_pan_tilt_data &speed);
-
- /**
- * \brief Function to move the pan-tilt a specific angle relative to the current position by a specific speed.
- * If some of the values passed are out of range, they'll be changed to the value of the limit.
- *
- * \param angle The specific angle in degrees to move from the current position for both servos in a struct.
- * It's value is changed to the goal position.
- * \param speed The specific speed in degrees/second for both servos in a struct.
- */
- void move_relative_angle(Dynamixel_pan_tilt_data &angle, Dynamixel_pan_tilt_data &speed);
-
- /**
- * \brief Function to move the pan-tilt on endless mode for both servos.
- * If some of the values passed are out of range, they'll be changed to the value of the limit.
- *
- * \param speed A struct with the specific speeds for the movemenet. It's a % of the maximum torque.
- */
- void move_torque(Dynamixel_pan_tilt_data &speed);
-
- /**
- * \brief Function to move the pan servo to a specific position by a specific speed.
- * If some of the values passed are out of range, they'll be changed to the value of the limit.
- *
- * \param angle The specific postion in degrees.
- * \param speed The specific speed for the movement in degrees/second.
- */
- void move_absolute_angle_pan(double &angle, double &speed);
-
- /**
- * \brief Function to move the pan servo a specific angle relative to the current position by a specific speed.
- * If some of the values passed are out of range, they'll be changed to the value of the limit.
- *
- * \param angle The specific angle to move from the current position in degrees.
- It's value is changed to the goal position.
- * \param speed The specific speed of the movement in degrees/second.
- */
- void move_relative_angle_pan(double &angle, double &speed);
-
- /**
- * \brief Function to move the pan servo on endless mode.
- * If the value passed is out of range, it'll be changed to the value of the limit.
- *
- * \param speed The specific speed for the movemenet. It's a % of the maximum torque.
- */
- void move_torque_pan(double &speed);
-
- /**
- * \brief Function to move the pan servo to a specific position by a specific speed.
- * If some of the values passed are out of range, they'll be changed to the value of the limit.
- *
- * \param angle The specific postion in degrees.
- * \param speed The specific speed for the movement in degrees/second.
- */
- void move_absolute_angle_tilt(double &angle, double &speed);
-
- /**
- * \brief Function to move the tilt servo a specific angle relative to the current position by a specific speed.
- * If some of the values passed are out of range, they'll be changed to the value of the limit.
- *
- * \param angle The specific angle to move from the current position in degrees.
- It's value is changed to the goal position.
- * \param speed The specific speed of the movement in degrees/second.
- */
- void move_relative_angle_tilt(double &angle, double &speed);
-
- /**
- * \brief Function to move the tilt servo on endless mode.
- * If the value passed is out of range, it'll be changed to the value of the limit.
- *
- * \param speed The specific speed for the movemenet. It's a % of the maximum torque.
- */
- void move_torque_tilt(double &speed);
-
- /**
- * \brief Function to stop the current movement of the pan-tilt.
- *
- * It always call the torque_movement function with a zero as argument. That's beacuse the servo can be out of range
- * at the moment of the call so it can generate a exception if it's on angle control mode.
- *
- */
- void stop(void);
-
- /**
- * \brief Function to get the current speed of both servos in degrees/s.
- *
- * \return A struct with the current speed of both servos in degrees/s.
- */
- Dynamixel_pan_tilt_data get_current_speed(void);
-
- /**
- * \brief Function to get the current effort of both servos in %.
- *
- * \return A struct with the current effort of both servos in %.
- */
- Dynamixel_pan_tilt_data get_current_effort(void);
-
- /**
- * \brief Function to get the compliance configuration of both servos.
- *
- * \param compliance The variable to save the configuration data.
- */
- void get_compliance_control(Dynamixel_pan_tilt_compliance &compliance);
-
- /**
- * \brief Function to set the compliance configuration of both servos.
- *
- * \param compliance The variable to get the configuration data.
- */
- void set_compliance_control(Dynamixel_pan_tilt_compliance &compliance);
-
- /**
- * \brief Function to get the PID values of both servos.
- *
- * \param pid The variable to save the PID values.
- */
- void get_pid_control(Dynamixel_pan_tilt_pid &pid);
-
- /**
- * \brief Function to set the PID values of both servos.
- *
- * \param pid The variable to get the PID values.
- */
- 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 Function to get the current max angle limit of both servos in deg.
- *
- * \return A struct with the current max angle limit of both servos in deg.
- */
- Dynamixel_pan_tilt_data get_max_angle_limit(void);
-
- /**
- * \brief Function to get the current min angle limit of both servos in deg.
- *
- * \return A struct with the current min angle limit of both servos in deg.
- */
- Dynamixel_pan_tilt_data get_min_angle_limit(void);
-
- /**
- * \brief User firendly function to print the absolute position of the pan-tilt.
- *
- */
- void print_current_position(void);
-
- /**
- * \brief User friendly function to print any pan-tilt data.
- *
- * \param data The data to be printed.
- */
- void print_pan_tilt_data(Dynamixel_pan_tilt_data data);
+
+ /**
+ * \brief Function to get the position of both servos in degrees.
+ *
+ * \return A struct with the angles of both servos in degrees.
+ */
+ Dynamixel_pan_tilt_data get_position(void);
+
+ /**
+ * \brief Function to get the position of the pan servo in degrees.
+ *
+ * \return The angle of the pan servo in degrees.
+ */
+ double get_pan_position(void);
+
+ /**
+ * \brief Function to get the position of the tilt servo in degrees.
+ *
+ * \return The angle of the tilt servo in degrees.
+ */
+ double get_tilt_position(void);
+
+ /**
+ * \brief Function to move the pan-tilt to a specific position by a specific speed.
+ * If some of the values passed are out of range, they'll be changed to the value of the limit.
+ *
+ * \param angle The specific position in degrees for both servos in a struct.
+ * \param speed The specific speed in degrees/second for both servos in a struct.
+ */
+ void move_absolute_angle(Dynamixel_pan_tilt_data &angle, Dynamixel_pan_tilt_data &speed);
+
+ /**
+ * \brief Function to move the pan-tilt a specific angle relative to the current position by a specific speed.
+ * If some of the values passed are out of range, they'll be changed to the value of the limit.
+ *
+ * \param angle The specific angle in degrees to move from the current position for both servos in a struct.
+ * It's value is changed to the goal position.
+ * \param speed The specific speed in degrees/second for both servos in a struct.
+ */
+ void move_relative_angle(Dynamixel_pan_tilt_data &angle, Dynamixel_pan_tilt_data &speed);
+
+ /**
+ * \brief Function to move the pan-tilt on endless mode for both servos.
+ * If some of the values passed are out of range, they'll be changed to the value of the limit.
+ *
+ * \param speed A struct with the specific speeds for the movemenet. It's a % of the maximum torque.
+ */
+ void move_torque(Dynamixel_pan_tilt_data &speed);
+
+ /**
+ * \brief Function to move the pan servo to a specific position by a specific speed.
+ * If some of the values passed are out of range, they'll be changed to the value of the limit.
+ *
+ * \param angle The specific postion in degrees.
+ * \param speed The specific speed for the movement in degrees/second.
+ */
+ void move_absolute_angle_pan(double &angle, double &speed);
+
+ /**
+ * \brief Function to move the pan servo a specific angle relative to the current position by a specific speed.
+ * If some of the values passed are out of range, they'll be changed to the value of the limit.
+ *
+ * \param angle The specific angle to move from the current position in degrees.
+ It's value is changed to the goal position.
+ * \param speed The specific speed of the movement in degrees/second.
+ */
+ void move_relative_angle_pan(double &angle, double &speed);
+
+ /**
+ * \brief Function to move the pan servo on endless mode.
+ * If the value passed is out of range, it'll be changed to the value of the limit.
+ *
+ * \param speed The specific speed for the movemenet. It's a % of the maximum torque.
+ */
+ void move_torque_pan(double &speed);
+
+ /**
+ * \brief Function to move the pan servo to a specific position by a specific speed.
+ * If some of the values passed are out of range, they'll be changed to the value of the limit.
+ *
+ * \param angle The specific postion in degrees.
+ * \param speed The specific speed for the movement in degrees/second.
+ */
+ void move_absolute_angle_tilt(double &angle, double &speed);
+
+ /**
+ * \brief Function to move the tilt servo a specific angle relative to the current position by a specific speed.
+ * If some of the values passed are out of range, they'll be changed to the value of the limit.
+ *
+ * \param angle The specific angle to move from the current position in degrees.
+ It's value is changed to the goal position.
+ * \param speed The specific speed of the movement in degrees/second.
+ */
+ void move_relative_angle_tilt(double &angle, double &speed);
+
+ /**
+ * \brief Function to move the tilt servo on endless mode.
+ * If the value passed is out of range, it'll be changed to the value of the limit.
+ *
+ * \param speed The specific speed for the movemenet. It's a % of the maximum torque.
+ */
+ void move_torque_tilt(double &speed);
+
+ /**
+ * \brief Function to stop the current movement of the pan-tilt.
+ *
+ * It always call the torque_movement function with a zero as argument. That's beacuse the servo can be out of range
+ * at the moment of the call so it can generate a exception if it's on angle control mode.
+ *
+ */
+ void stop(void);
+
+ /**
+ * \brief Function to get the current speed of both servos in degrees/s.
+ *
+ * \return A struct with the current speed of both servos in degrees/s.
+ */
+ Dynamixel_pan_tilt_data get_current_speed(void);
+
+ /**
+ * \brief Function to get the current effort of both servos in %.
+ *
+ * \return A struct with the current effort of both servos in %.
+ */
+ Dynamixel_pan_tilt_data get_current_effort(void);
+
+ /**
+ * \brief Function to get the compliance configuration of both servos.
+ *
+ * \param compliance The variable to save the configuration data.
+ */
+ void get_compliance_control(Dynamixel_pan_tilt_compliance &compliance);
+
+ /**
+ * \brief Function to set the compliance configuration of both servos.
+ *
+ * \param compliance The variable to get the configuration data.
+ */
+ void set_compliance_control(Dynamixel_pan_tilt_compliance &compliance);
+
+ /**
+ * \brief Function to get the PID values of both servos.
+ *
+ * \param pid The variable to save the PID values.
+ */
+ void get_pid_control(Dynamixel_pan_tilt_pid &pid);
+
+ /**
+ * \brief Function to set the PID values of both servos.
+ *
+ * \param pid The variable to get the PID values.
+ */
+ 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 Function to get the current max angle limit of both servos in deg.
+ *
+ * \return A struct with the current max angle limit of both servos in deg.
+ */
+ Dynamixel_pan_tilt_data get_max_angle_limit(void);
+
+ /**
+ * \brief Function to get the current min angle limit of both servos in deg.
+ *
+ * \return A struct with the current min angle limit of both servos in deg.
+ */
+ Dynamixel_pan_tilt_data get_min_angle_limit(void);
+
+ /**
+ * \brief User firendly function to print the absolute position of the pan-tilt.
+ *
+ */
+ void print_current_position(void);
+
+ /**
+ * \brief User friendly function to print any pan-tilt data.
+ *
+ * \param data The data to be printed.
+ */
+ void print_pan_tilt_data(Dynamixel_pan_tilt_data data);
};
diff --git a/src/examples/test_dynamixel_pan_tilt.cpp b/src/examples/test_dynamixel_pan_tilt.cpp
index de44d89..40ffb00 100644
--- a/src/examples/test_dynamixel_pan_tilt.cpp
+++ b/src/examples/test_dynamixel_pan_tilt.cpp
@@ -128,7 +128,7 @@ std::string config_saving_file = "../src/xml/saving_config.xml";
int main(int argc, char *argv[])
{
- std::string serial="AI027ZIZ"; //extracted from with 'dmesg' command: "SerialNumber: AI027ZIZ"
+ std::string serial="AI027ZIZ"; //extracted from with 'dmesg' command: "SerialNumber: AI027ZIZ"
int baudrate = 1000000; //57600 or 1000000
int pan_id = 13; //extracted from dynamixel library's test_dynamixel_server_no_scan
int tilt_id = 11; //extracted from dynamixel library's test_dynamixel_server_no_scan
@@ -429,7 +429,7 @@ int main(int argc, char *argv[])
}
else
{
- std::cout << "No buses!!! tip: sudo rmmod ftdi_sio" << std::endl;
+ std::cout << "No buses!!! tip: sudo rmmod ftdi_sio" << std::endl;
}
}
catch(CException &e)
--
GitLab