Merge pull request #361 from snozawa/add_stop_start_sync_mode

Add stop start sync mode
fkanehiro · Oct 10, 2014 · d808c5a · d808c5a
2 parents 1657376 + 02769b5
commit d808c5a
Show file tree

Hide file tree

Showing 5 changed files with 28 additions and 12 deletions.
diff --git a/rtc/AutoBalancer/AutoBalancer.cpp b/rtc/AutoBalancer/AutoBalancer.cpp
@@ -276,12 +276,17 @@ RTC::ReturnCode_t AutoBalancer::onActivated(RTC::UniqueId ec_id)
     return RTC::RTC_OK;
 }
 
-/*
-  RTC::ReturnCode_t AutoBalancer::onDeactivated(RTC::UniqueId ec_id)
-  {
-  return RTC::RTC_OK;
+RTC::ReturnCode_t AutoBalancer::onDeactivated(RTC::UniqueId ec_id)
+{
+  std::cout << "AutoBalancer::onDeactivated(" << ec_id << ")" << std::endl;
+  Guard guard(m_mutex);
+  if (control_mode == MODE_ABC) {
+    stopABCparam();
+    control_mode = MODE_IDLE;
+    transition_count = 1; // sync in one controller loop
   }
-*/
+  return RTC::RTC_OK;
+}
 
 #define DEBUGP ((m_debugLevel==1 && loop%200==0) || m_debugLevel > 1 )
 //#define DEBUGP2 ((loop%200==0))

diff --git a/rtc/AutoBalancer/AutoBalancer.h b/rtc/AutoBalancer/AutoBalancer.h
@@ -64,7 +64,7 @@ class AutoBalancer
 
   // The deactivated action (Active state exit action)
   // former rtc_active_exit()
-  // virtual RTC::ReturnCode_t onDeactivated(RTC::UniqueId ec_id);
+  virtual RTC::ReturnCode_t onDeactivated(RTC::UniqueId ec_id);
 
   // The execution action that is invoked periodically
   // former rtc_active_do()

diff --git a/rtc/ImpedanceController/ImpedanceController.cpp b/rtc/ImpedanceController/ImpedanceController.cpp
@@ -206,12 +206,16 @@ RTC::ReturnCode_t ImpedanceController::onActivated(RTC::UniqueId ec_id)
     return RTC::RTC_OK;
 }
 
-/*
-  RTC::ReturnCode_t ImpedanceController::onDeactivated(RTC::UniqueId ec_id)
-  {
-  return RTC::RTC_OK;
+RTC::ReturnCode_t ImpedanceController::onDeactivated(RTC::UniqueId ec_id)
+{
+  std::cout << "ImpedanceController::onDeactivated(" << ec_id << ")" << std::endl;
+  Guard guard(m_mutex);
+  for ( std::map<std::string, ImpedanceParam>::iterator it = m_impedance_param.begin(); it != m_impedance_param.end(); it++ ) {
+    deleteImpedanceController(it->first);
+    m_impedance_param[it->first].transition_count = 1;
   }
-*/
+  return RTC::RTC_OK;
+}
 
 #define DEBUGP ((m_debugLevel==1 && loop%200==0) || m_debugLevel > 1 )
 RTC::ReturnCode_t ImpedanceController::onExecute(RTC::UniqueId ec_id)

diff --git a/rtc/ImpedanceController/ImpedanceController.h b/rtc/ImpedanceController/ImpedanceController.h
@@ -63,7 +63,7 @@ class ImpedanceController
 
   // The deactivated action (Active state exit action)
   // former rtc_active_exit()
-  // virtual RTC::ReturnCode_t onDeactivated(RTC::UniqueId ec_id);
+  virtual RTC::ReturnCode_t onDeactivated(RTC::UniqueId ec_id);
 
   // The execution action that is invoked periodically
   // former rtc_active_do()

diff --git a/rtc/Stabilizer/Stabilizer.cpp b/rtc/Stabilizer/Stabilizer.cpp
@@ -301,6 +301,11 @@ RTC::ReturnCode_t Stabilizer::onActivated(RTC::UniqueId ec_id)
 RTC::ReturnCode_t Stabilizer::onDeactivated(RTC::UniqueId ec_id)
 {
   std::cout << m_profile.instance_name<< ": onDeactivated(" << ec_id << ")" << std::endl;
+  if ( (control_mode == MODE_ST || control_mode == MODE_AIR) ) {
+    sync_2_idle ();
+    control_mode = MODE_IDLE;
+    transition_count = 1; // sync in one controller loop
+  }
   return RTC::RTC_OK;
 }
 
@@ -1275,6 +1280,7 @@ RTC::ReturnCode_t Stabilizer::onRateChanged(RTC::UniqueId ec_id)
 
 void Stabilizer::sync_2_st ()
 {
+  std::cerr << "Sync IDLE => ST"  << std::endl;
   pangx_ref = pangy_ref = pangx = pangy = 0;
   rdx = rdy = rx = ry = 0;
   d_rpy[0] = d_rpy[1] = 0;
@@ -1292,6 +1298,7 @@ void Stabilizer::sync_2_st ()
 
 void Stabilizer::sync_2_idle ()
 {
+  std::cerr << "Sync ST => IDLE"  << std::endl;
   transition_count = MAX_TRANSITION_COUNT;
   for (int i = 0; i < m_robot->numJoints(); i++ ) {
     transition_joint_q[i] = m_robot->joint(i)->q;