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