diff --git a/src/dynamixel_pan_tilt.cpp b/src/dynamixel_pan_tilt.cpp
index 54663e3b9e58404652800fdbae63bd67456e5e75..4143cbe1d6b132b17a8031f1f373f9c9fe8a14c9 100644
--- a/src/dynamixel_pan_tilt.cpp
+++ b/src/dynamixel_pan_tilt.cpp
@@ -72,13 +72,13 @@ CDynamixel_Pan_Tilt::CDynamixel_Pan_Tilt(std::string& name_pan_tilt, CDynamixelS
exit(EXIT_FAILURE);
}
- actualice_parameters();
+ update_parameters();
}
catch(CException &e)
{
/* handle exceptions */
- std::cout << "Exception on the constructor." << std::endl;
- std::cout << e.what() << std::endl;
+ std::cout << "\033[31m Exception on the constructor." << std::endl;
+ std::cout << e.what() << "\033[0m" << std::endl;
if(this->pan!=NULL)
{
delete this->pan;
@@ -169,12 +169,13 @@ void CDynamixel_Pan_Tilt::load_config(Dynamixel_pan_tilt_config &config)
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;
+ update_angle_limits();
}
catch (CException &e)
{
- actualice_config();
+ update_config();
get_pan_tilt_config(config);
- std::cout << "Loading config exception: " << e.what() << std::endl;
+ std::cout << "\033[31m Loading config exception: " << e.what() << "\033[0m" << std::endl;
throw CDynamixel_Pan_TiltException(_HERE_, "The dynamixel pan&tilt config hasn't be loaded properly.");
}
}
@@ -324,8 +325,8 @@ void CDynamixel_Pan_Tilt::load_config(std::string &filename)
}
else
{
- this->checking_hysteresis_offset.pan = 2.404867;
- this->checking_hysteresis_offset.tilt = 2.404867;
+ this->checking_hysteresis_offset.pan = 0.0;//2.404867;
+ this->checking_hysteresis_offset.tilt = 0.0;//2.404867;
}
if (cfg->check_hyst_slope().size() > 0)
@@ -342,7 +343,7 @@ void CDynamixel_Pan_Tilt::load_config(std::string &filename)
this->checking_hysteresis_slope.tilt = 0.20688;
}
- actualice_parameters();
+ update_parameters();
}
catch (const xml_schema::exception& e)
{
@@ -410,8 +411,9 @@ void CDynamixel_Pan_Tilt::read_config(std::string &filename, Dynamixel_pan_tilt_
void CDynamixel_Pan_Tilt::save_config(std::string &filename)
{
+ std::cout << "\033[33m WARNING: this function doesn't do anynothing \033[0m" << std::endl;
/*xml_schema::namespace_infomap map;
- actualice_parameters();
+ update_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));
@@ -484,7 +486,6 @@ 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);
}
@@ -495,36 +496,33 @@ void CDynamixel_Pan_Tilt::move_absolute_angle_pan(double &angle, double &speed)
{
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);
+ if (this->mode.pan != angle_ctrl)
+ {//It has to update the normal limits and the mode
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->config.pan.min_angle >= angle)
+ {
+ std::cout << "\033[33m WARNING: The minimum pan's angle is " << this->config.pan.min_angle << "; Moving pan to this angle. \033[0m" << std::endl;
+ angle = this->config.pan.min_angle + ANGLE_LIMIT_CORRECTION;//on the exact limit it gives an error beacuse the conversion between angle and the word
+ }
+ else if (this->config.pan.max_angle <= angle)
+ {
+ std::cout << "\033[33m WARNING: The maximum pan's angle is " << this->config.pan.max_angle << "; Moving pan to this angle. \033[0m" << std::endl;
+ angle = this->config.pan.max_angle - ANGLE_LIMIT_CORRECTION;//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)
+ {
+ std::cout << "\033[33m WARNING: The maximum pan' speed is " << this->info.pan.max_speed*this->config.pan.max_torque/100 << "; Moving pan at this speed. \033[0m" << std::endl;
+ speed = this->info.pan.max_speed*this->config.pan.max_torque/100;
+ }
+
+ set_moving_state_pan(stopped);
+ this->pan->move_absolute_angle(angle, speed);
if (this->event_server->event_is_set(this->torque_moving))
this->event_server->reset_event(this->torque_moving);
@@ -532,7 +530,7 @@ void CDynamixel_Pan_Tilt::move_absolute_angle_pan(double &angle, double &speed)
}
catch(CException &e)
{
- std::cout << "move pan absolute Exception: ";
+ std::cout << "\033[31m Move pan absolute Exception: " << e.what() << "\033[0m" << std::endl;
this->mode.pan = this->pan->get_control_mode();
this->pan->get_position_range(&this->config.pan.min_angle, &this->config.pan.max_angle);
stop();
@@ -553,44 +551,40 @@ void CDynamixel_Pan_Tilt::move_absolute_angle_tilt(double &angle, double &speed)
{
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);
+ if (this->mode.tilt != angle_ctrl)
+ {//It has to update the normal limits and the mode
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->config.tilt.min_angle >= angle)
+ {
+ std::cout << "\033[33m WARNING: The minimum tilt's angle is " << this->config.tilt.min_angle << "; Moving tilt to this angle. \033[0m" << std::endl;
+ angle = this->config.tilt.min_angle + ANGLE_LIMIT_CORRECTION;//on the exact limit it gives an error beacuse the conversion between angle and the word
+ }
+ else if (this->config.tilt.max_angle <= angle)
+ {
+ std::cout << "\033[33m WARNING: The maximum tilt's angle is " << this->config.tilt.max_angle << "; Moving tilt to this angle. \033[0m" << std::endl;
+ angle = this->config.tilt.max_angle - ANGLE_LIMIT_CORRECTION;//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)
+ {
+ std::cout << "\033[33m WARNING: The maximum tilt' speed is " << this->info.tilt.max_speed*this->config.tilt.max_torque/100 << "; Moving tilt at this speed. \033[0m" << std::endl;
+ speed = this->info.tilt.max_speed*this->config.tilt.max_torque/100;
+ }
+
+ set_moving_state_tilt(stopped);
+ this->tilt->move_absolute_angle(angle, speed);
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: ";
+ std::cout << "\033[31m Move tilt absolute Exception: " << e.what() << "\033[0m" << std::endl;
this->mode.tilt = this->tilt->get_control_mode();
this->tilt->get_position_range(&this->config.tilt.min_angle, &this->config.tilt.max_angle);
stop();
@@ -622,7 +616,7 @@ void CDynamixel_Pan_Tilt::move_relative_angle_pan(double &angle, double &speed)
{
angle = 0.0;
speed = 0.0;
- throw CDynamixel_Pan_TiltException(_HERE_,"The dynamixel tilt servo is not properly configured. Not moving");
+ throw CDynamixel_Pan_TiltException(_HERE_,"The dynamixel pan servo is not properly configured. Not moving");
}
}
@@ -651,77 +645,143 @@ void CDynamixel_Pan_Tilt::move_torque_pan(double &speed)
{
if (this->pan!=NULL)
{
+ set_moving_state_pan(stopped);
if (fabs(speed) > this->config.pan.max_torque)
+ {
(speed > 0 ? speed = this->config.pan.max_torque : speed = -(this->config.pan.max_torque));
+ std::cout << "\033[33m WARNING: The maximum pan' speed is " << this->config.pan.max_torque << "; Moving pan at this speed. \033[0m" << std::endl;
+ }
set_checking_hysteresis_pan(calculate_checking_hysteresis_pan(fabs(speed)));
+ // double hyst_aux = 0.0;
+ // bool change_ctrl_mode = (this->mode.pan == angle_ctrl);
+ if (this->mode.pan == angle_ctrl)
+ {
+ 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;
+ }
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))
+ double angle_diff = this->max_angle.pan - pos;
+ double eff = max_safe_effort_pan(angle_diff);
+ // std::cout << "effort calculated " << eff << std::endl;
+ if (fabs(eff) > fabs(speed))
+ eff = speed;
+ try
{
- 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;
- }
+ set_moving_state_pan(positive_torque);
+ this->pan->move_torque(eff);
+ if (!this->event_server->event_is_set(this->torque_moving))
+ this->event_server->set_event(this->torque_moving);
}
- else
+ catch (CException &e)
{
- 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);
+ std::cout << "\033[31m move pan torque Exception: " << e.what() << "\033[0m" << std::endl;
+ 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;
}
+ // 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 << "\033[31m move pan torque Exception: " << e.what() << "\033[0m" << std::endl;
+ // 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
+ // {
+ // set_moving_state_pan(stopped);
+ // double sp = 0;
+ // this->pan->move_torque(sp);
+ // if (!this->event_server->event_is_set(this->torque_moving))
+ // this->event_server->set_event(this->torque_moving);
+ // // if (change_ctrl_mode) change_ctrl_mode = !change_ctrl_mode;
+ // // 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))
+ double angle_diff = pos - this->min_angle.pan;
+ double eff = max_safe_effort_pan(angle_diff);
+ // std::cout << "effort calculated " << eff << std::endl;
+ if (fabs(eff) > fabs(speed))
+ eff = speed;
+ else
+ eff = -eff;
+ try
{
- 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;
- }
+ set_moving_state_pan(negative_torque);
+ this->pan->move_torque(eff);
+ if (!this->event_server->event_is_set(this->torque_moving))
+ this->event_server->set_event(this->torque_moving);
}
- else
+ catch (CException &e)
{
- 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);
+ std::cout << "\033[31m move pan torque Exception: " << e.what() << "\033[0m" << std::endl;
+ 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;
}
+ // 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 << "\033[31m move pan torque Exception: " << e.what() << "\033[0m" << std::endl;
+ // 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
+ // {
+ // set_moving_state_pan(stopped);
+ // double sp = 0;
+ // this->pan->move_torque(sp);
+ // if (!this->event_server->event_is_set(this->torque_moving))
+ // this->event_server->set_event(this->torque_moving);
+ // // if (change_ctrl_mode) change_ctrl_mode = !change_ctrl_mode;
+ // // 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
{
@@ -734,7 +794,7 @@ void CDynamixel_Pan_Tilt::move_torque_pan(double &speed)
}
catch (CException &e)
{
- std::cout << "move pan torque Exception: ";
+ std::cout << "\033[31m move pan torque Exception: " << e.what() << "\033[0m" << std::endl;
stop();
this->mode.pan = this->pan->get_control_mode();
this->pan->get_position_range(&this->config.pan.min_angle, &this->config.pan.max_angle);
@@ -744,12 +804,12 @@ void CDynamixel_Pan_Tilt::move_torque_pan(double &speed)
}
}
- 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;
- }
+ // if (change_ctrl_mode)
+ // {
+ // 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
{
@@ -762,77 +822,144 @@ void CDynamixel_Pan_Tilt::move_torque_tilt(double &speed)
{
if (this->tilt!=NULL)
{
+ set_moving_state_tilt(stopped);
if (fabs(speed) > this->config.tilt.max_torque)
+ {
(speed > 0 ? speed = this->config.tilt.max_torque : speed = -(this->config.tilt.max_torque));
+ std::cout << "\033[33m WARNING: The maximum tilt' speed is " << this->config.tilt.max_torque << "; Moving tilt at this speed. \033[0m" << std::endl;
+ }
set_checking_hysteresis_tilt(calculate_checking_hysteresis_tilt(fabs(speed)));
+ //double hyst_aux = 0.0;
+ //bool change_ctrl_mode = (this->mode.tilt == angle_ctrl);
+ if (this->mode.tilt == angle_ctrl)
+ {
+ 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;
+ }
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))
+ //get_checking_hysteresis_tilt(hyst_aux);
+ double angle_diff = this->max_angle.tilt - pos;
+ double eff = max_safe_effort_tilt(angle_diff);
+ // std::cout << "effort calculated " << eff << std::endl;
+ if (fabs(eff) > fabs(speed))
+ eff = speed;
+ try
{
- 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;
- }
+ set_moving_state_tilt(positive_torque);
+ this->tilt->move_torque(eff);
+ if (!this->event_server->event_is_set(this->torque_moving))
+ this->event_server->set_event(this->torque_moving);
}
- else
+ catch (CException &e)
{
- 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);
+ std::cout << "\033[31m move tilt torque Exception: " << e.what() << "\033[0m" << std::endl;
+ 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 (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 << "\033[31m move tilt torque Exception: " << e.what() << "\033[0m" << std::endl;
+ // 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
+ // {
+ // set_moving_state_tilt(stopped);
+ // double sp = 0;
+ // this->tilt->move_torque(sp);
+ // if (!this->event_server->event_is_set(this->torque_moving))
+ // this->event_server->set_event(this->torque_moving);
+ // // if (change_ctrl_mode) change_ctrl_mode = !change_ctrl_mode;
+ // // 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))
+ double angle_diff = pos - this->min_angle.tilt;
+ double eff = max_safe_effort_tilt(angle_diff);
+ // std::cout << "effort calculated " << eff << std::endl;
+ if (fabs(eff) > fabs(speed))
+ eff = speed;
+ else
+ eff = -eff;
+ try
{
- 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;
- }
+ set_moving_state_tilt(negative_torque);
+ this->tilt->move_torque(eff);
+ if (!this->event_server->event_is_set(this->torque_moving))
+ this->event_server->set_event(this->torque_moving);
}
- else
+ catch (CException &e)
{
- 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);
+ std::cout << "\033[31m move tilt torque Exception: " << e.what() << "\033[0m" << std::endl;
+ 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;
}
+ // 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 << "\033[31m move tilt torque Exception: " << e.what() << "\033[0m" << std::endl;
+ // 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
+ // {
+ // set_moving_state_tilt(stopped);
+ // double sp = 0;
+ // this->tilt->move_torque(sp);
+ // if (!this->event_server->event_is_set(this->torque_moving))
+ // this->event_server->set_event(this->torque_moving);
+ // // if (change_ctrl_mode) change_ctrl_mode = !change_ctrl_mode;
+ // // 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
{
@@ -845,7 +972,7 @@ void CDynamixel_Pan_Tilt::move_torque_tilt(double &speed)
}
catch (CException &e)
{
- std::cout << "move tilt torque Exception: ";
+ std::cout << "\033[31m move tilt torque Exception: " << e.what() << "\033[0m" << std::endl;
stop();
this->mode.tilt = this->tilt->get_control_mode();
this->tilt->get_position_range(&this->config.tilt.min_angle, &this->config.tilt.max_angle);
@@ -854,12 +981,12 @@ void CDynamixel_Pan_Tilt::move_torque_tilt(double &speed)
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;
- }
+ // if (change_ctrl_mode)
+ // {
+ // 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
{
@@ -906,8 +1033,24 @@ void *CDynamixel_Pan_Tilt::checking_angle_limits(void *arg)
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);
+ double angle_diff = pan_tilt->max_angle.pan - pos.pan;
+ double eff = pan_tilt->max_safe_effort_pan(angle_diff);
+ eff *= CHECKING_SPEED_FACTOR;
+ // std::cout << "pan effort calculated " << eff << std::endl;
+
+ try
+ {
+ pan_tilt->pan->move_torque(eff);
+ }
+ catch (CException &e)
+ {
+ std::cout << "\033[31m move pan torque Exception on checking_angle_limits: " << e.what() << "\033[0m" << std::endl;
+ pan_tilt->stop();
+ throw;
+ }
+
+ // pan_tilt->pan->stop();
+ // pan_tilt->set_moving_state_pan(stopped);
//pan_tilt->checking_hysteresis.pan = 0.0;
// std::cout << "pan out!" << std::endl;
@@ -915,8 +1058,23 @@ void *CDynamixel_Pan_Tilt::checking_angle_limits(void *arg)
}
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);
+ double angle_diff = pos.pan - pan_tilt->min_angle.pan;
+ double eff = pan_tilt->max_safe_effort_pan(angle_diff);
+ eff *= -CHECKING_SPEED_FACTOR;
+ // std::cout << "pan effort calculated " << eff << std::endl;
+
+ try
+ {
+ pan_tilt->pan->move_torque(eff);
+ }
+ catch (CException &e)
+ {
+ std::cout << "\033[31m move pan torque Exception on checking_angle_limits: " << e.what() << "\033[0m" << std::endl;
+ pan_tilt->stop();
+ throw;
+ }
+ // pan_tilt->pan->stop();
+ // pan_tilt->set_moving_state_pan(stopped);
//pan_tilt->checking_hysteresis.pan = 0.0;
// std::cout << "pan out!" << std::endl;
@@ -926,8 +1084,23 @@ void *CDynamixel_Pan_Tilt::checking_angle_limits(void *arg)
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);
+ double angle_diff = pan_tilt->max_angle.tilt - pos.tilt;
+ double eff = pan_tilt->max_safe_effort_tilt(angle_diff);
+ eff *= CHECKING_SPEED_FACTOR;
+ // std::cout << "tilt effort calculated " << eff << std::endl;
+
+ try
+ {
+ pan_tilt->tilt->move_torque(eff);
+ }
+ catch (CException &e)
+ {
+ std::cout << "\033[31m move tilt torque Exception on checking_angle_limits: " << e.what() << "\033[0m" << std::endl;
+ pan_tilt->stop();
+ throw;
+ }
+ // pan_tilt->tilt->stop();
+ // pan_tilt->set_moving_state_tilt(stopped);
//pan_tilt->checking_hysteresis.tilt = 0.0;
// std::cout << "tilt out!" << std::endl;
@@ -935,8 +1108,23 @@ void *CDynamixel_Pan_Tilt::checking_angle_limits(void *arg)
}
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);
+ double angle_diff = pos.tilt - pan_tilt->min_angle.tilt;
+ double eff = pan_tilt->max_safe_effort_tilt(angle_diff);
+ eff *= -CHECKING_SPEED_FACTOR;
+ // std::cout << "tilt effort calculated " << eff << std::endl;
+
+ try
+ {
+ pan_tilt->tilt->move_torque(eff);
+ }
+ catch (CException &e)
+ {
+ std::cout << "\033[31m move tilt torque Exception on checking_angle_limits: " << e.what() << "\033[0m" << std::endl;
+ pan_tilt->stop();
+ throw;
+ }
+ // pan_tilt->tilt->stop();
+ // pan_tilt->set_moving_state_tilt(stopped);
//pan_tilt->checking_hysteresis.tilt = 0.0;
// std::cout << "tilt out!" << std::endl;
@@ -955,7 +1143,8 @@ void *CDynamixel_Pan_Tilt::checking_angle_limits(void *arg)
}
catch (CException &e)
{
- std::cout << "Checking angle limits exception: " << e.what() << std::endl;
+ std::cout << "\033[31m Checking angle limits exception: " << e.what() << "\033[0m" << std::endl;
+ throw;
}
}
}
@@ -970,7 +1159,7 @@ void CDynamixel_Pan_Tilt::stop(void)
}
catch (CException &e)
{
- std::cout << "stop Exception: ";
+ std::cout << "\033[31m Stop Exception: " << e.what() << "\033[0m" << std::endl;
//stop();
throw;
}
@@ -985,19 +1174,19 @@ void CDynamixel_Pan_Tilt::stop(void)
///------------------------------------------------------Movement
-///------------------------------------------------Actualice servo parameters
+///------------------------------------------------update servo parameters
-void CDynamixel_Pan_Tilt::actualice_parameters(void)
+void CDynamixel_Pan_Tilt::update_parameters(void)
{
- actualice_info();
- actualice_pid_control();
- actualice_compliance_control();
- actualice_config();
- actualice_control_mode();
- actualice_angle_limits();
+ update_info();
+ update_pid_control();
+ update_compliance_control();
+ update_config();
+ update_control_mode();
+ update_angle_limits();
}
-void CDynamixel_Pan_Tilt::actualice_config(void)
+void CDynamixel_Pan_Tilt::update_config(void)
{
if (this->pan!=NULL && this->tilt!=NULL)
{
@@ -1017,15 +1206,15 @@ void CDynamixel_Pan_Tilt::actualice_config(void)
}
catch(CDynamixelAlarmException &e)
{
- std::cout << "actualice config Exception: " << e.what() << std::endl;
+ std::cout << "\033[31m Update config Exception: " << e.what() << "\033[0m" << std::endl;
throw;
}
}
else
- throw CDynamixel_Pan_TiltException(_HERE_,"The dynamixel pan&tilt is not properly configured. Config not actualiced");
+ throw CDynamixel_Pan_TiltException(_HERE_,"The dynamixel pan&tilt is not properly configured. Config not updated");
}
-void CDynamixel_Pan_Tilt::actualice_angle_limits(void)
+void CDynamixel_Pan_Tilt::update_angle_limits(void)
{
this->min_angle.pan = this->config.pan.min_angle;
this->max_angle.pan = this->config.pan.max_angle;
@@ -1033,7 +1222,7 @@ void CDynamixel_Pan_Tilt::actualice_angle_limits(void)
this->max_angle.tilt = this->config.tilt.max_angle;
}
-void CDynamixel_Pan_Tilt::actualice_control_mode(void)
+void CDynamixel_Pan_Tilt::update_control_mode(void)
{
if (this->pan!=NULL && this->tilt!=NULL)
{
@@ -1044,15 +1233,15 @@ void CDynamixel_Pan_Tilt::actualice_control_mode(void)
}
catch(CDynamixelAlarmException &e)
{
- std::cout << "actualice control mode Exception: " << e.what() << std::endl;
+ std::cout << "\033[31m Update control mode Exception: " << e.what() << "\033[0m" << std::endl;
throw;
}
}
else
- throw CDynamixel_Pan_TiltException(_HERE_,"The dynamixel pan&tilt is not properly configured. Config not actualiced");
+ throw CDynamixel_Pan_TiltException(_HERE_,"The dynamixel pan&tilt is not properly configured. Config not updated");
}
-void CDynamixel_Pan_Tilt::actualice_pid_control(void)
+void CDynamixel_Pan_Tilt::update_pid_control(void)
{
if (this->pan!=NULL && this->tilt!=NULL)
{
@@ -1063,15 +1252,15 @@ void CDynamixel_Pan_Tilt::actualice_pid_control(void)
}
catch(CDynamixelAlarmException &e)
{
- std::cout << "actualice pid control exception: " << e.what() << std::endl;
+ std::cout << "\033[31m Update pid control exception: " << e.what() << "\033[0m" << std::endl;
throw;
}
}
else
- throw CDynamixel_Pan_TiltException(_HERE_,"The dynamixel pan&tilt is not properly configured. PID control not actualiced");
+ throw CDynamixel_Pan_TiltException(_HERE_,"The dynamixel pan&tilt is not properly configured. PID control not updated");
}
-void CDynamixel_Pan_Tilt::actualice_info(void)
+void CDynamixel_Pan_Tilt::update_info(void)
{
if (this->pan!=NULL && this->tilt!=NULL)
{
@@ -1082,15 +1271,15 @@ void CDynamixel_Pan_Tilt::actualice_info(void)
}
catch(CDynamixelAlarmException &e)
{
- std::cout << "actualice info exception: " << e.what() << std::endl;
+ std::cout << "\033[31m Update info exception: " << e.what() << "\033[0m" << std::endl;
throw;
}
}
else
- throw CDynamixel_Pan_TiltException(_HERE_,"The dynamixel pan&tilt is not properly configured. Info not actualiced");
+ throw CDynamixel_Pan_TiltException(_HERE_,"The dynamixel pan&tilt is not properly configured. Info not updated");
}
-void CDynamixel_Pan_Tilt::actualice_compliance_control(void)
+void CDynamixel_Pan_Tilt::update_compliance_control(void)
{
if (this->pan!=NULL && this->tilt!=NULL)
{
@@ -1101,12 +1290,12 @@ void CDynamixel_Pan_Tilt::actualice_compliance_control(void)
}
catch(CDynamixelAlarmException &e)
{
- std::cout << "actualice compliance control exception: " << e.what() << std::endl;
+ std::cout << "\033[31m Update compliance control exception: " << e.what() << "\033[0m" << std::endl;
throw;
}
}
else
- throw CDynamixel_Pan_TiltException(_HERE_,"The dynamixel pan&tilt is not properly configured. Compliance control not actualiced");
+ throw CDynamixel_Pan_TiltException(_HERE_,"The dynamixel pan&tilt is not properly configured. Compliance control not updated");
}
void CDynamixel_Pan_Tilt::default_parameters(void)
@@ -1172,7 +1361,7 @@ void CDynamixel_Pan_Tilt::default_parameters(void)
}
-///------------------------------------------------Actualice servo parameters
+///------------------------------------------------update servo parameters
///------------------------------------------------------Get & Set
@@ -1196,12 +1385,12 @@ double CDynamixel_Pan_Tilt::get_pan_position(void)
}
catch(CException &e)
{
- std::cout << "get pan position exception: " << e.what() << std::endl;
+ std::cout << "\033[31m Get pan position exception: " << e.what() << "\033[0m" << std::endl;
throw;
}
}
else
- throw CDynamixel_Pan_TiltException(_HERE_,"The dynamixel pan servo is not properly configured. Position not actualiced");
+ throw CDynamixel_Pan_TiltException(_HERE_,"The dynamixel pan servo is not properly configured. Position not updated");
}
double CDynamixel_Pan_Tilt::get_tilt_position(void)
@@ -1214,12 +1403,12 @@ double CDynamixel_Pan_Tilt::get_tilt_position(void)
}
catch(CException &e)
{
- std::cout << "get tilt position exception: " << e.what() << std::endl;
+ std::cout << "\033[31m Get tilt position exception: " << e.what() << "\033[0m" << std::endl;
throw;
}
}
else
- throw CDynamixel_Pan_TiltException(_HERE_,"The dynamixel tilt servo is not properly configured. Position not actualiced");
+ throw CDynamixel_Pan_TiltException(_HERE_,"The dynamixel tilt servo is not properly configured. Position not updated");
}
void CDynamixel_Pan_Tilt::get_compliance_control(Dynamixel_pan_tilt_compliance &compliance)
@@ -1239,8 +1428,8 @@ void CDynamixel_Pan_Tilt::set_compliance_control(Dynamixel_pan_tilt_compliance &
}
catch (CException &e)
{
- std::cout << "set compliance control exception: " << e.what() << std::endl;
- actualice_compliance_control();
+ std::cout << "\033[31m Set compliance control exception: " << e.what() << "\033[0m" << std::endl;
+ update_compliance_control();
compliance = this->compliance;
throw CDynamixel_Pan_TiltException(_HERE_, "The dynamixel pan&tilt compliance control hasn't been set properly.");
}
@@ -1269,8 +1458,8 @@ void CDynamixel_Pan_Tilt::set_pid_control(Dynamixel_pan_tilt_pid &pid)
}
catch (CException &e)
{
- std::cout << "set pid control exception: " << e.what() << std::endl;
- actualice_pid_control();
+ std::cout << "\033[31m Set pid control exception: " << e.what() << "\033[0m" << std::endl;
+ update_pid_control();
pid = this->pid;
throw CDynamixel_Pan_TiltException(_HERE_, "The dynamixel pan&tilt PID control hasn't been set properly.");
}
@@ -1405,7 +1594,7 @@ double CDynamixel_Pan_Tilt::calculate_checking_hysteresis_pan(double effort)
}
else
{
- return this->checking_hysteresis_slope.pan*effort - checking_hysteresis_offset.pan;
+ return this->checking_hysteresis_slope.pan*effort + this->checking_hysteresis_offset.pan;
}
}
@@ -1417,10 +1606,26 @@ double CDynamixel_Pan_Tilt::calculate_checking_hysteresis_tilt(double effort)
}
else
{
- return this->checking_hysteresis_slope.tilt*effort - checking_hysteresis_offset.tilt;
+ return this->checking_hysteresis_slope.tilt*effort + this->checking_hysteresis_offset.tilt;
}
}
+double CDynamixel_Pan_Tilt::max_safe_effort_pan(double angle_to_limit)
+{
+ if (angle_to_limit < 0.0)
+ return 0.0;
+ else
+ return (angle_to_limit - this->checking_hysteresis_offset.pan)/this->checking_hysteresis_slope.pan;
+}
+
+double CDynamixel_Pan_Tilt::max_safe_effort_tilt(double angle_to_limit)
+{
+ if (angle_to_limit < 0.0)
+ return 0.0;
+ else
+ return (angle_to_limit - this->checking_hysteresis_offset.tilt)/this->checking_hysteresis_slope.tilt;
+}
+
Dynamixel_pan_tilt_data CDynamixel_Pan_Tilt::get_current_speed(void)
{
if (this->pan!=NULL && this->tilt!=NULL)
@@ -1434,7 +1639,7 @@ Dynamixel_pan_tilt_data CDynamixel_Pan_Tilt::get_current_speed(void)
}
catch(CException &e)
{
- std::cout << "get current speed exception: " << e.what() << std::endl;
+ std::cout << "\033[31m Get current speed exception: " << e.what() << "\033[0m" << std::endl;
throw;
}
}
@@ -1455,7 +1660,7 @@ Dynamixel_pan_tilt_data CDynamixel_Pan_Tilt::get_current_effort(void)
}
catch(CException &e)
{
- std::cout << "get current effort exception: " << e.what() << std::endl;
+ std::cout << "\033[31m Get current effort exception: " << e.what() << "\033[0m" << std::endl;
throw;
}
}
@@ -1476,7 +1681,7 @@ Dynamixel_pan_tilt_data CDynamixel_Pan_Tilt::get_current_effort(void)
}
catch(CException &e)
{
- std::cout << "set max torque exception: " << e.what() << std::endl;
+ std::cout << "\033[31m Set max torque exception: " << e.what() << "\033[0m" << 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.");
@@ -1503,7 +1708,7 @@ Dynamixel_pan_tilt_data CDynamixel_Pan_Tilt::get_limit_torque(void)
}
catch(CException &e)
{
- std::cout << "get limit torque exception: " << e.what() << std::endl;
+ std::cout << "\033[31m Get limit torque exception: " << e.what() << "\033[0m" << std::endl;
throw;
}
}
@@ -1560,7 +1765,7 @@ void CDynamixel_Pan_Tilt::print_current_position(void)
}
catch(CException &e)
{
- std::cout << "print current position exception: " << e.what() << std::endl;
+ std::cout << "\033[31m Print current position exception: " << e.what() << "\033[0m" << std::endl;
throw;
}
}
diff --git a/src/dynamixel_pan_tilt.h b/src/dynamixel_pan_tilt.h
index ddf5917794393d985db303281c38af737e479395..0518ba987c7f6043201e9418f32b01931a88e3e5 100644
--- a/src/dynamixel_pan_tilt.h
+++ b/src/dynamixel_pan_tilt.h
@@ -33,6 +33,8 @@
#include "eventexceptions.h"
#define CHECKING_TIME_INTERVAL 1000
+#define ANGLE_LIMIT_CORRECTION 0.05
+#define CHECKING_SPEED_FACTOR 0.5
/**
* \struct Dynamixel_pan_tilt_data.
@@ -204,6 +206,8 @@ 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_angle_torque;
+ Dynamixel_pan_tilt_data min_angle_torque;
}Dynamixel_pan_tilt_xml_limits;
@@ -278,46 +282,46 @@ class CDynamixel_Pan_Tilt
void default_parameters(void);
/**
- * \brief Function to actualice all the variables with all the parameters of both servos.
+ * \brief Function to update all the variables with all the parameters of both servos.
*
*/
- void actualice_parameters(void);
+ void update_parameters(void);
/**
- * \brief Function to actualice the variable of configuration of both servos.
+ * \brief Function to update the variable of configuration of both servos.
*
*/
- void actualice_config(void);
+ void update_config(void);
/**
- * \brief Function to actualice the variable of compliance control of the pan-tilt.
+ * \brief Function to update the variable of compliance control of the pan-tilt.
*
*/
- void actualice_compliance_control(void);
+ void update_compliance_control(void);
/**
- * \brief Function to actualice the variable of the angle limits of the pan-tilt.
+ * \brief Function to update the variable of the angle limits of the pan-tilt.
*
*/
- void actualice_angle_limits(void);
+ void update_angle_limits(void);
/**
- * \brief Function to actualice the variable of control mode of the pan-tilt.
+ * \brief Function to update the variable of control mode of the pan-tilt.
*
*/
- void actualice_control_mode(void);
+ void update_control_mode(void);
/**
- * \brief Function to actualice the variable of pid control of the pan-tilt.
+ * \brief Function to update the variable of pid control of the pan-tilt.
*
*/
- void actualice_pid_control(void);
+ void update_pid_control(void);
/**
- * \brief Function to actualice the variable with the information of the pan-tilt.
+ * \brief Function to update the variable with the information of the pan-tilt.
*
*/
- void actualice_info(void);
+ void update_info(void);
/**
* \brief Function to be called for the thread in charge to check the angle limits on torque movement.
@@ -385,6 +389,26 @@ class CDynamixel_Pan_Tilt
*/
double calculate_checking_hysteresis_tilt(double effort);
+ /**
+ * \brief Function to calculate the maximum effort to don't overcome the limits, depending on the distance to
+ * the limit of the pan servo.
+ *
+ * \param angle_to_limit The current ditance to the limit of the pan servo.
+ *
+ * \return The maximum effort.
+ */
+ double max_safe_effort_pan(double angle_to_limit);
+
+ /**
+ * \brief Function to calculate the maximum effort to don't overcome the limits, depending on the distance to
+ * the limit of the tilt servo.
+ *
+ * \param angle_to_limit The current ditance to the limit of the tilt servo.
+ *
+ * \return The maximum effort.
+ */
+ double max_safe_effort_tilt(double angle_to_limit);
+
/**
* \brief Function to get the value of pan's hysteresis.
*
@@ -433,7 +457,7 @@ class 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.
+ * If some parameters are changed internally, it's updated on the config variable.
*
* \param config The configuration structure.
*/
diff --git a/src/examples/test_dynamixel_pan_tilt.cpp b/src/examples/test_dynamixel_pan_tilt.cpp
index 40ffb00e5ad3aa3ed858bf427a79dbdf4c161bf6..9ff7e47be41d06a7ff13df5f79a4986ae84bcf71 100644
--- a/src/examples/test_dynamixel_pan_tilt.cpp
+++ b/src/examples/test_dynamixel_pan_tilt.cpp
@@ -124,14 +124,15 @@
std::string name="PAN_TILT_AX_12plus";
std::string config_file="../src/xml/dyn_pan_tilt_config_AX12plus.xml";
+//std::string config_file="../src/xml/dyn_pan_tilt_config_RX64.xml";
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="A4012AD3"; //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
+ int pan_id = 1; //extracted from dynamixel library's test_dynamixel_server_no_scan
+ int tilt_id = 2; //extracted from dynamixel library's test_dynamixel_server_no_scan
CDynamixelServerFTDI *dyn_server=CDynamixelServerFTDI::instance();
CDynamixel_Pan_Tilt *pan_tilt = NULL;
@@ -224,9 +225,9 @@ int main(int argc, char *argv[])
double time_interval = 0.1; //time in secs between checks
double max_time_sec = 5.0; //max time to wait until timeout
double t;
- double uperiod = time_interval*1000000.0;
+ double uperiod = time_interval*1000000.0/2.0;
double timeout = max_time_sec/time_interval;
- double torque_timeout = 45.0;
+ double torque_timeout = 85.0;
desired_speed.pan = 354;
desired_speed.tilt = 354;
@@ -238,8 +239,8 @@ int main(int argc, char *argv[])
std::cout << "------------------------MOVE ABSOLUTE--------------------------" << std::endl;
pos = pan_tilt->get_position();
- desired_pos.pan = -80.1613;
- desired_pos.tilt = -80.1613;
+ desired_pos.pan = 60;
+ desired_pos.tilt = 60;
std::cout << "Desired position: ";
std::cout << desired_pos;
//pan_tilt->print_pan_tilt_data(desired_pos);
@@ -297,8 +298,8 @@ int main(int argc, char *argv[])
std::cout << desired_effort;
//pan_tilt->print_pan_tilt_data(desired_effort);
- pan_tilt->move_torque(desired_effort);
+ pan_tilt->move_torque(desired_effort);
std::cout << "Moving..." << std::endl;
std::cout << "Moving effort: ";
@@ -310,6 +311,8 @@ int main(int argc, char *argv[])
{
//pos = pan_tilt->get_position();
//pan_tilt->print_current_position();
+ // pan_tilt->move_torque(desired_effort);
+ // pan_tilt->print_current_position();
usleep(uperiod);
t++;
}
@@ -387,8 +390,8 @@ int main(int argc, char *argv[])
std::cout << std::endl << std::endl;
std::cout << "------------------------MOVE TORQUE--------------------------" << std::endl;
- desired_effort.pan = -75;
- desired_effort.tilt = -75;
+ desired_effort.pan = -80;
+ desired_effort.tilt = -80;
std::cout << "Current angle: ";
pan_tilt->print_current_position();
@@ -426,6 +429,129 @@ int main(int argc, char *argv[])
sleep(1);
//*/
+ std::cout << std::endl << std::endl;
+ std::cout << "------------------------MOVE TORQUE--------------------------" << std::endl;
+
+ desired_effort.pan = 60;
+ desired_effort.tilt = 60;
+
+ std::cout << "Current angle: ";
+ pan_tilt->print_current_position();
+
+ std::cout << "Desired effort: ";
+ std::cout << desired_effort;
+ //pan_tilt->print_pan_tilt_data(desired_effort);
+
+ pan_tilt->move_torque(desired_effort);
+
+ std::cout << "Moving..." << std::endl;
+
+ std::cout << "Moving effort: ";
+ pan_tilt->print_pan_tilt_data(desired_effort);
+
+ //torque_timeout = 45.0;
+ t=0.0;
+ while(t<torque_timeout)
+ {
+ //pos = pan_tilt->get_position();
+ //pan_tilt->print_current_position();
+ usleep(uperiod);
+ t++;
+ }
+
+ pan_tilt->stop();
+
+ std::cout << "Reached position: ";
+ pan_tilt->print_current_position();
+
+ pan_tilt->get_pan_tilt_config(config);
+
+ std::cout << "[INFO]: Angle range PAN: (" << config.pan.min_angle << "," << config.pan.max_angle << ")" << std::endl;
+ std::cout << "[INFO]: Angle range TILT: (" << config.tilt.min_angle << "," << config.tilt.max_angle << ")" << std::endl;
+ sleep(1);
+
+ std::cout << std::endl << std::endl;
+ std::cout << "------------------------MOVE TORQUE--------------------------" << std::endl;
+
+ desired_effort.pan = -40;
+ desired_effort.tilt = -40;
+
+ std::cout << "Current angle: ";
+ pan_tilt->print_current_position();
+
+ std::cout << "Desired effort: ";
+ std::cout << desired_effort;
+ //pan_tilt->print_pan_tilt_data(desired_effort);
+
+ pan_tilt->move_torque(desired_effort);
+
+ std::cout << "Moving..." << std::endl;
+
+ std::cout << "Moving effort: ";
+ pan_tilt->print_pan_tilt_data(desired_effort);
+
+ //torque_timeout = 45.0;
+ t=0.0;
+ while(t<torque_timeout)
+ {
+ //pos = pan_tilt->get_position();
+ //pan_tilt->print_current_position();
+ usleep(uperiod);
+ t++;
+ }
+
+ pan_tilt->stop();
+
+ std::cout << "Reached position: ";
+ pan_tilt->print_current_position();
+
+ pan_tilt->get_pan_tilt_config(config);
+
+ std::cout << "[INFO]: Angle range PAN: (" << config.pan.min_angle << "," << config.pan.max_angle << ")" << std::endl;
+ std::cout << "[INFO]: Angle range TILT: (" << config.tilt.min_angle << "," << config.tilt.max_angle << ")" << std::endl;
+ sleep(1);
+
+ std::cout << std::endl << std::endl;
+ std::cout << "------------------------MOVE TORQUE--------------------------" << std::endl;
+
+ desired_effort.pan = 20;
+ desired_effort.tilt = 20;
+
+ std::cout << "Current angle: ";
+ pan_tilt->print_current_position();
+
+ std::cout << "Desired effort: ";
+ std::cout << desired_effort;
+ //pan_tilt->print_pan_tilt_data(desired_effort);
+
+ pan_tilt->move_torque(desired_effort);
+
+ std::cout << "Moving..." << std::endl;
+
+ std::cout << "Moving effort: ";
+ pan_tilt->print_pan_tilt_data(desired_effort);
+
+ //torque_timeout = 45.0;
+ t=0.0;
+ while(t<torque_timeout)
+ {
+ //pos = pan_tilt->get_position();
+ //pan_tilt->print_current_position();
+ usleep(uperiod);
+ t++;
+ }
+
+ pan_tilt->stop();
+
+ std::cout << "Reached position: ";
+ pan_tilt->print_current_position();
+
+ pan_tilt->get_pan_tilt_config(config);
+
+ std::cout << "[INFO]: Angle range PAN: (" << config.pan.min_angle << "," << config.pan.max_angle << ")" << std::endl;
+ std::cout << "[INFO]: Angle range TILT: (" << config.tilt.min_angle << "," << config.tilt.max_angle << ")" << std::endl;
+ sleep(1);
+
}
else
{
diff --git a/src/xml/dyn_pan_tilt_config_AX12plus.xml b/src/xml/dyn_pan_tilt_config_AX12plus.xml
index 7e98cd5481f82d6cd1593f4090011b55ada85ed2..08f72b6a0c45133b2f4cf83d18e6068c0d4994fd 100644
--- a/src/xml/dyn_pan_tilt_config_AX12plus.xml
+++ b/src/xml/dyn_pan_tilt_config_AX12plus.xml
@@ -4,10 +4,10 @@
xsi:noNamespaceSchemaLocation="dynamixel_pan_tilt_cfg_file.xsd">
<alarm_shtdwn>5</alarm_shtdwn>
- <temp_limit>85</temp_limit>
+ <temp_limit>90</temp_limit>
<max_voltage>19</max_voltage>
<min_voltage>6</min_voltage>
- <max_torque>60</max_torque>
+ <max_torque>100</max_torque>
<cw_comp_margin>2</cw_comp_margin>
<ccw_comp_margin>2</ccw_comp_margin>
<cw_comp_slope>64</cw_comp_slope>