From 4c790905d405c9a84c403650f0820c5154b1f115 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20L=C3=B3pez=20Gestoso?= <alopez@iri.upc.edu> Date: Fri, 15 Apr 2016 08:23:34 +0000 Subject: [PATCH] Fixed some minor errors --- src/dynamixel_pan_tilt.cpp | 332 +++++++++++++++-------- src/dynamixel_pan_tilt.h | 60 +++- src/examples/test_dynamixel_pan_tilt.cpp | 18 +- src/xml/dyn_pan_tilt_config_AX12plus.xml | 2 +- 4 files changed, 283 insertions(+), 129 deletions(-) diff --git a/src/dynamixel_pan_tilt.cpp b/src/dynamixel_pan_tilt.cpp index bd8a289..589940b 100644 --- a/src/dynamixel_pan_tilt.cpp +++ b/src/dynamixel_pan_tilt.cpp @@ -34,8 +34,8 @@ CDynamixel_Pan_Tilt::CDynamixel_Pan_Tilt(std::string& name_pan_tilt, CDynamixelS this->pan = NULL; this->tilt = NULL; reset_torque_moving_state(); - this->checking_hysteresis.pan = 0.0; - this->checking_hysteresis.tilt = 0.0; + 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; @@ -93,8 +93,6 @@ CDynamixel_Pan_Tilt::CDynamixel_Pan_Tilt(std::string& name_pan_tilt, CDynamixelS throw; } - - } CDynamixel_Pan_Tilt::~CDynamixel_Pan_Tilt() @@ -139,11 +137,35 @@ CDynamixel_Pan_Tilt::~CDynamixel_Pan_Tilt() void CDynamixel_Pan_Tilt::load_config(Dynamixel_pan_tilt_config &config) { - this->pan->load_config(config.pan); - this->tilt->load_config(config.tilt); + 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); + } + - actualice_config(); - get_pan_tilt_config(config); + } #ifdef _HAVE_XSD @@ -404,6 +426,7 @@ void CDynamixel_Pan_Tilt::move_absolute_angle_pan(double &angle, double &speed) 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 @@ -416,23 +439,24 @@ void CDynamixel_Pan_Tilt::move_absolute_angle_pan(double &angle, double &speed) if (fabs(speed) > this->info.pan.max_speed*this->config.pan.max_torque/100) speed = this->info.pan.max_speed*this->config.pan.max_torque/100; - this->pan->move_absolute_angle(angle, speed); - this->pan->set_position_range(this->min_angle.pan, this->max_angle.pan); + + set_moving_state_pan(stopped); + this->pan->move_absolute_angle(angle, speed); this->mode.pan = this->pan->get_control_mode(); this->pan->get_position_range(&this->config.pan.min_angle, &this->config.pan.max_angle); } - set_moving_state_pan(stopped); + if (this->event_server->event_is_set(this->torque_moving)) { this->event_server->reset_event(this->torque_moving); } - this->checking_hysteresis.pan = 0.0; + set_checking_hysteresis_pan(0.0); } catch(CException &e) { - //std::cout << e.what() << std::endl; + std::cout << e.what() << std::endl; } } @@ -450,6 +474,7 @@ void CDynamixel_Pan_Tilt::move_absolute_angle_tilt(double &angle, double &speed) 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 @@ -462,23 +487,24 @@ void CDynamixel_Pan_Tilt::move_absolute_angle_tilt(double &angle, double &speed) if (fabs(speed) > this->info.tilt.max_speed*this->config.tilt.max_torque/100) speed = this->info.tilt.max_speed*this->config.tilt.max_torque/100; - this->tilt->move_absolute_angle(angle, speed); - this->tilt->set_position_range(this->min_angle.tilt, this->max_angle.tilt); + + set_moving_state_tilt(stopped); + this->tilt->move_absolute_angle(angle, speed); this->mode.tilt = this->tilt->get_control_mode(); this->tilt->get_position_range(&this->config.tilt.min_angle, &this->config.tilt.max_angle); } - set_moving_state_tilt(stopped); + if (this->event_server->event_is_set(this->torque_moving)) { this->event_server->reset_event(this->torque_moving); } - this->checking_hysteresis.tilt = 0.0; + set_checking_hysteresis_tilt(0.0); } catch(CException &e) { - //std::cout << e.what() << std::endl; + std::cout << e.what() << std::endl; } } @@ -512,19 +538,24 @@ void CDynamixel_Pan_Tilt::move_torque_pan(double &speed) { if (fabs(speed) > this->config.pan.max_torque) (speed > 0 ? speed = this->config.pan.max_torque : speed = -(this->config.pan.max_torque)); - this->checking_hysteresis.pan = get_checking_hysteresis_pan(fabs(speed)); - Dynamixel_pan_tilt_data pos = get_position(); + 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) { - if (pos.pan < (this->max_angle.pan - this->checking_hysteresis.pan)) + get_checking_hysteresis_pan(hyst_aux); + if (pos < (this->max_angle.pan - hyst_aux)) { this->pan->move_torque(speed); set_moving_state_pan(positive_torque); - this->event_server->set_event(this->torque_moving); + if (!this->event_server->event_is_set(this->torque_moving)) + this->event_server->set_event(this->torque_moving); } 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; this->move_absolute_angle_pan(angle, aux_speed); @@ -532,14 +563,17 @@ void CDynamixel_Pan_Tilt::move_torque_pan(double &speed) } else if (speed < 0) { - if (pos.pan > (this->min_angle.pan + this->checking_hysteresis.pan)) + get_checking_hysteresis_pan(hyst_aux); + if (pos > (this->min_angle.pan + hyst_aux)) { this->pan->move_torque(speed); set_moving_state_pan(negative_torque); - this->event_server->set_event(this->torque_moving); + if (!this->event_server->event_is_set(this->torque_moving)) + this->event_server->set_event(this->torque_moving); } 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; this->move_absolute_angle_pan(angle, aux_speed); @@ -549,18 +583,19 @@ void CDynamixel_Pan_Tilt::move_torque_pan(double &speed) { set_moving_state_pan(stopped); if (this->event_server->event_is_set(this->torque_moving)) - { this->event_server->reset_event(this->torque_moving); - } this->pan->move_torque(speed); } - this->mode.pan = this->pan->get_control_mode(); - this->pan->get_position_range(&this->config.pan.min_angle, &this->config.pan.max_angle); + if (ctrl_mode_changed) + { + this->mode.pan = this->pan->get_control_mode(); + this->pan->get_position_range(&this->config.pan.min_angle, &this->config.pan.max_angle); + } } catch (CException &e) { - //std::cout << e.what() << std::endl; + std::cout << e.what() << std::endl; } } @@ -570,19 +605,24 @@ void CDynamixel_Pan_Tilt::move_torque_tilt(double &speed) { if (fabs(speed) > this->config.tilt.max_torque) (speed > 0 ? speed = this->config.tilt.max_torque : speed = -(this->config.tilt.max_torque)); - this->checking_hysteresis.tilt = get_checking_hysteresis_tilt(fabs(speed)); - Dynamixel_pan_tilt_data pos = get_position(); + 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) { - if (pos.tilt < (this->max_angle.tilt - this->checking_hysteresis.tilt)) + get_checking_hysteresis_tilt(hyst_aux); + if (pos < (this->max_angle.tilt - hyst_aux)) { this->tilt->move_torque(speed); set_moving_state_tilt(positive_torque); - this->event_server->set_event(this->torque_moving); + if (!this->event_server->event_is_set(this->torque_moving)) + this->event_server->set_event(this->torque_moving); } 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; this->move_absolute_angle_tilt(angle, aux_speed); @@ -590,14 +630,17 @@ void CDynamixel_Pan_Tilt::move_torque_tilt(double &speed) } else if (speed < 0) { - if (pos.tilt > (this->min_angle.tilt + this->checking_hysteresis.tilt)) + get_checking_hysteresis_tilt(hyst_aux); + if (pos > (this->min_angle.tilt + hyst_aux)) { this->tilt->move_torque(speed); set_moving_state_tilt(negative_torque); - this->event_server->set_event(this->torque_moving); + if (!this->event_server->event_is_set(this->torque_moving)) + this->event_server->set_event(this->torque_moving); } 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; this->move_absolute_angle_tilt(angle, aux_speed); @@ -607,13 +650,14 @@ void CDynamixel_Pan_Tilt::move_torque_tilt(double &speed) { set_moving_state_tilt(stopped); if (this->event_server->event_is_set(this->torque_moving)) - { this->event_server->reset_event(this->torque_moving); - } this->tilt->move_torque(speed); } - this->mode.tilt = this->tilt->get_control_mode(); - this->tilt->get_position_range(&this->config.tilt.min_angle, &this->config.tilt.max_angle); + if (ctrl_mode_changed) + { + this->mode.tilt = this->tilt->get_control_mode(); + this->tilt->get_position_range(&this->config.tilt.min_angle, &this->config.tilt.max_angle); + } } catch (CException &e) { @@ -623,6 +667,7 @@ void CDynamixel_Pan_Tilt::move_torque_tilt(double &speed) 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(); @@ -634,72 +679,81 @@ void *CDynamixel_Pan_Tilt::checking_angle_limits(void *arg) while (true) { - event_id = event_server->wait_first(event_list); - - if (event_id == 0)//== EXITING - { - pthread_exit(NULL); - } - else if (event_id == 1)// ==TORQUE_MOVING + try { - 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); - - while (is_moving) + event_id = event_server->wait_first(event_list); + if (event_id == 0)//== EXITING { - pos = pan_tilt->get_position(); - if ((pan_state == positive_torque) && (pos.pan >= (pan_tilt->max_angle.pan - pan_tilt->checking_hysteresis.pan))) - { - 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 + pan_tilt->checking_hysteresis.pan))) - { - 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); - } - - if ((tilt_state == positive_torque) && (pos.tilt >= (pan_tilt->max_angle.tilt - pan_tilt->checking_hysteresis.tilt))) - { - 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 + pan_tilt->checking_hysteresis.tilt))) - { - 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); + 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); - if (event_server->event_is_set(pan_tilt->exiting)) + double hyst_aux = 0.0; + + while (is_moving) { - pthread_exit(NULL); + 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; + } } } @@ -708,10 +762,12 @@ void CDynamixel_Pan_Tilt::stop(void) Dynamixel_pan_tilt_data dat; dat.pan = 0.0; dat.tilt = 0.0; + //this->pan->stop(); + //this->tilt->stop(); move_torque(dat); reset_torque_moving_state(); - this->checking_hysteresis.pan = 0.0; - this->checking_hysteresis.tilt = 0.0; + set_checking_hysteresis_pan(0.0); + set_checking_hysteresis_tilt(0.0); } @@ -867,8 +923,8 @@ void CDynamixel_Pan_Tilt::default_parameters(void) this->event_server->reset_event(this->torque_moving); } reset_torque_moving_state(); - this->checking_hysteresis.pan = 0.0; - this->checking_hysteresis.tilt = 0.0; + 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; @@ -918,7 +974,6 @@ double CDynamixel_Pan_Tilt::get_tilt_position(void) void CDynamixel_Pan_Tilt::get_compliance_control(Dynamixel_pan_tilt_compliance &compliance) { - actualice_compliance_control(); compliance = this->compliance; } @@ -928,27 +983,19 @@ void CDynamixel_Pan_Tilt::set_compliance_control(Dynamixel_pan_tilt_compliance & { this->pan->set_compliance_control(compliance.pan); this->tilt->set_compliance_control(compliance.tilt); - actualice_compliance_control(); - compliance = this->compliance; + this->compliance = compliance; } catch (CException &e) { - + actualice_compliance_control(); + compliance = this->compliance; } } void CDynamixel_Pan_Tilt::get_pid_control(Dynamixel_pan_tilt_pid &pid) { - try - { - actualice_pid_control(); - pid = this->pid; - } - catch (CException &e) - { - - } + pid = this->pid; } void CDynamixel_Pan_Tilt::set_pid_control(Dynamixel_pan_tilt_pid &pid) @@ -957,12 +1004,12 @@ void CDynamixel_Pan_Tilt::set_pid_control(Dynamixel_pan_tilt_pid &pid) { this->pan->set_pid_control(pid.pan); this->tilt->set_pid_control(pid.tilt); - actualice_pid_control(); - pid = this->pid; + this->pid = pid; } catch (CException &e) { - + actualice_pid_control(); + pid = this->pid; } } @@ -1053,7 +1100,35 @@ void CDynamixel_Pan_Tilt::get_moving_state_tilt(Torque_moving_state &tms) pthread_mutex_unlock(&(this->mut)); } -double CDynamixel_Pan_Tilt::get_checking_hysteresis_pan(double effort) +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)); +} + +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)); +} + +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)); +} + +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)); +} + +double CDynamixel_Pan_Tilt::calculate_checking_hysteresis_pan(double effort) { if (effort == 0.0) { @@ -1065,7 +1140,7 @@ double CDynamixel_Pan_Tilt::get_checking_hysteresis_pan(double effort) } } -double CDynamixel_Pan_Tilt::get_checking_hysteresis_tilt(double effort) +double CDynamixel_Pan_Tilt::calculate_checking_hysteresis_tilt(double effort) { if (effort == 0.0) { @@ -1077,10 +1152,31 @@ double CDynamixel_Pan_Tilt::get_checking_hysteresis_tilt(double effort) } } +Dynamixel_pan_tilt_data CDynamixel_Pan_Tilt::get_current_speed(void) +{ + Dynamixel_pan_tilt_data data; + data.pan = this->pan->get_current_speed(); + data.tilt = this->tilt->get_current_speed(); + return data; +} + +Dynamixel_pan_tilt_data CDynamixel_Pan_Tilt::get_current_effort(void) +{ + Dynamixel_pan_tilt_data data; + data.pan = this->pan->get_current_effort(); + data.tilt = this->tilt->get_current_effort(); + return data; +} + ///------------------------------------------------------Get & Set ///--------------------------------------------------------Prints +std::ostream& operator<< (std::ostream& out, Dynamixel_pan_tilt_data &data) +{ + out << "(" << data.pan << " , " << data.tilt << ")" << std::endl; + return out; +} void CDynamixel_Pan_Tilt::print_current_position(void) { print_pan_tilt_data(this->get_position()); diff --git a/src/dynamixel_pan_tilt.h b/src/dynamixel_pan_tilt.h index 89b218a..a3bdfa7 100644 --- a/src/dynamixel_pan_tilt.h +++ b/src/dynamixel_pan_tilt.h @@ -326,7 +326,7 @@ class CDynamixel_Pan_Tilt void set_moving_state_pan(Torque_moving_state tms); /** - * \brief Function to get a value on pan's moving state. + * \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. */ @@ -340,7 +340,7 @@ class CDynamixel_Pan_Tilt void set_moving_state_tilt(Torque_moving_state tms); /** - * \brief Function to get a value on tilt's moving state. + * \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. */ @@ -353,7 +353,7 @@ class CDynamixel_Pan_Tilt * * \return The hysteresis angle for the pan servo. */ - double get_checking_hysteresis_pan(double effort); + double calculate_checking_hysteresis_pan(double effort); /** * \brief Function to calculate the angle hysteresis depending on the desired effort for the tilt servo. @@ -362,7 +362,35 @@ class CDynamixel_Pan_Tilt * * \return The hysteresis angle for the tilt servo. */ - double get_checking_hysteresis_tilt(double effort); + 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: @@ -534,6 +562,20 @@ class CDynamixel_Pan_Tilt */ 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. * @@ -574,6 +616,16 @@ class CDynamixel_Pan_Tilt * \param data The data to be printed. */ void print_pan_tilt_data(Dynamixel_pan_tilt_data data); + }; +/** + * \brief Overload of the << operator to print easily any pan-tilt data. + * + * \param out The output stream. + * \param data The data to be printed. + * \return The output stream. + */ +std::ostream& operator<< (std::ostream& out, Dynamixel_pan_tilt_data &data); + #endif diff --git a/src/examples/test_dynamixel_pan_tilt.cpp b/src/examples/test_dynamixel_pan_tilt.cpp index 16e39b9..225426b 100644 --- a/src/examples/test_dynamixel_pan_tilt.cpp +++ b/src/examples/test_dynamixel_pan_tilt.cpp @@ -234,7 +234,8 @@ int main(int argc, char *argv[]) desired_pos.pan = -80.1613; desired_pos.tilt = -80.1613; std::cout << "Desired position: "; - pan_tilt->print_pan_tilt_data(desired_pos); + std::cout << desired_pos; + //pan_tilt->print_pan_tilt_data(desired_pos); std::cout << "Current position: "; pan_tilt->print_current_position(); pan_tilt->move_absolute_angle(desired_pos, desired_speed); @@ -256,7 +257,8 @@ int main(int argc, char *argv[]) } std::cout << "Desired position: "; - pan_tilt->print_pan_tilt_data(desired_pos); + std::cout << desired_pos; + //pan_tilt->print_pan_tilt_data(desired_pos); std::cout << "Reached position: "; pan_tilt->print_current_position(); std::cout << "Error position: (" << pos.pan - desired_pos.pan << " , " << pos.tilt - desired_pos.tilt << ")" << std::endl; @@ -285,7 +287,8 @@ int main(int argc, char *argv[]) pan_tilt->print_current_position(); std::cout << "Desired effort: "; - pan_tilt->print_pan_tilt_data(desired_effort); + std::cout << desired_effort; + //pan_tilt->print_pan_tilt_data(desired_effort); pan_tilt->move_torque(desired_effort); @@ -331,7 +334,8 @@ int main(int argc, char *argv[]) pan_tilt->move_relative_angle(desired_pos,desired_speed); std::cout << "Desired angle: "; - pan_tilt->print_pan_tilt_data(desired_pos); + std::cout << desired_pos; + //pan_tilt->print_pan_tilt_data(desired_pos); std::cout << "Moving..." << std::endl; @@ -353,7 +357,8 @@ int main(int argc, char *argv[]) } std::cout << "Desired position: "; - pan_tilt->print_pan_tilt_data(desired_pos); + std::cout << desired_pos; + //pan_tilt->print_pan_tilt_data(desired_pos); std::cout << "Reached position: "; pan_tilt->print_current_position(); std::cout << "Error position: (" << pos.pan - desired_pos.pan << " , " << pos.tilt - desired_pos.tilt << ")" << std::endl; @@ -382,7 +387,8 @@ int main(int argc, char *argv[]) pan_tilt->print_current_position(); std::cout << "Desired effort: "; - pan_tilt->print_pan_tilt_data(desired_effort); + std::cout << desired_effort; + //pan_tilt->print_pan_tilt_data(desired_effort); pan_tilt->move_torque(desired_effort); diff --git a/src/xml/dyn_pan_tilt_config_AX12plus.xml b/src/xml/dyn_pan_tilt_config_AX12plus.xml index 037093d..abc5082 100644 --- a/src/xml/dyn_pan_tilt_config_AX12plus.xml +++ b/src/xml/dyn_pan_tilt_config_AX12plus.xml @@ -7,7 +7,7 @@ <temp_limit>85</temp_limit> <max_voltage>19</max_voltage> <min_voltage>6</min_voltage> - <max_torque>100</max_torque> + <max_torque>20</max_torque> <cw_comp_margin>2</cw_comp_margin> <ccw_comp_margin>2</ccw_comp_margin> <cw_comp_slope>64</cw_comp_slope> -- GitLab