Fix PIAxis state machine

src/piaxis/AxisController.cpp

@@ -58,6 +58,8 @@ std::string AxisController::get_model()
 void AxisController::compute_state_status()
 {
     try
+    {
+        if(m_state != Tango::FAULT)
         {
             Serializer::pull_from_server(&m_on, m_axis_id);
             if (m_on.get_value() == 1)
@@ -69,7 +71,6 @@ void AxisController::compute_state_status()
                 {
                     set_state(Tango::MOVING);
                     set_status("Moving");
-                return;
                 }
             }
             else
@@ -77,6 +78,7 @@ void AxisController::compute_state_status()
                 set_state(Tango::OFF);
                 set_status("Motor off");
             }
+        }
         Serializer::pull_from_server(&m_is_initialized, m_axis_id); //Test
         if (m_is_initialized.get_value() == 0)
         {
@@ -381,6 +383,7 @@ void AxisController::process_message(yat::Message& msg)
             case CTRL_ON_MSG:
             {
                 //DEBUG_STREAM << "-> AxisController::TASK_CTRL_ON_MSG" << std::endl;
+                set_state(Tango::ON);
                 Serializer::push_to_server(&m_on, m_axis_id);
             }
                 break;
@@ -389,6 +392,7 @@ void AxisController::process_message(yat::Message& msg)
             case CTRL_OFF_MSG:
             {
                 DEBUG_STREAM << "-> AxisController::TASK_CTRL_OFF_MSG" << std::endl;
+                set_state(Tango::OFF);
                 Serializer::push_to_server(&m_off, m_axis_id);
             }
                 break;
@@ -396,7 +400,6 @@ void AxisController::process_message(yat::Message& msg)
 
             case CTRL_MODEL_MSG:
             {
-                //DEBUG_STREAM << "-> AxisController::TASK_CTRL_MODEL_MSG" << std::endl;
                 Serializer::pull_from_server(&m_model, m_axis_id);
             }
                 break;
@@ -405,6 +408,7 @@ void AxisController::process_message(yat::Message& msg)
             case CTRL_SETPOSITION_MSG:
             {
                 DEBUG_STREAM << "-> AxisController::TASK_CTRL_SETPOSITION_MSG" << std::endl;
+                set_state(Tango::MOVING);
                 Serializer::push_to_server(&m_position, m_axis_id);
             }
                 break;
@@ -412,7 +416,6 @@ void AxisController::process_message(yat::Message& msg)
             
             case CTRL_GETPOSITION_MSG:
             {
-                //DEBUG_STREAM << "-> AxisController::TASK_CTRL_GETPOSITION_MSG" << std::endl;
                 Serializer::pull_from_server(&m_position, m_axis_id);
             }
                 break;
@@ -428,7 +431,6 @@ void AxisController::process_message(yat::Message& msg)
             
             case CTRL_GETVELOCITY_MSG:
             {
-               // DEBUG_STREAM << "-> AxisController::TASK_CTRL_GETVELOCITY_MSG" << std::endl;
                 Serializer::pull_from_server(&m_velocity, m_axis_id);
             }
                 break;
@@ -444,7 +446,6 @@ void AxisController::process_message(yat::Message& msg)
             
             case CTRL_GETACCELERATION_MSG:
             {
-                //DEBUG_STREAM << "-> AxisController::TASK_CTRL_GETACCELERATION_MSG" << std::endl;
                 Serializer::pull_from_server(&m_acceleration, m_axis_id);
             }
                 break;
@@ -460,7 +461,6 @@ void AxisController::process_message(yat::Message& msg)
             
             case CTRL_GETDECELERATION_MSG:
             {
-                //DEBUG_STREAM << "-> AxisController::TASK_CTRL_GETDECELERATION_MSG" << std::endl;
                 Serializer::pull_from_server(&m_deceleration, m_axis_id);
             }
                 break;
@@ -468,7 +468,6 @@ void AxisController::process_message(yat::Message& msg)
             
             case CTRL_NLIMIT_MSG:
             {
-               // DEBUG_STREAM << "-> AxisController::TASK_CTRL_NLIMIT_MSG" << std::endl;
                 Serializer::pull_from_server(&m_limit_switch, m_axis_id);
             }
                 break;
@@ -476,7 +475,6 @@ void AxisController::process_message(yat::Message& msg)
             
             case CTRL_PLIMIT_MSG:
             {
-               // DEBUG_STREAM << "-> AxisController::TASK_CTRL_PLIMIT_MSG" << std::endl;
                 Serializer::pull_from_server(&m_limit_switch, m_axis_id);
             }
                 break;
@@ -485,6 +483,7 @@ void AxisController::process_message(yat::Message& msg)
             case CTRL_STOP_MSG:
             {
                 DEBUG_STREAM << "-> AxisController::TASK_CTRL_STOP_MSG" << std::endl;
+                set_state(Tango::OFF);
                 Serializer::push_to_server(&m_stop, m_axis_id);
             }
                 break;
@@ -493,6 +492,7 @@ void AxisController::process_message(yat::Message& msg)
             case CTRL_BACKWARD_MSG:
             {
                 DEBUG_STREAM << "-> AxisController::TASK_CTRL_BACKWARD_MSG" << std::endl;
+                set_state(Tango::MOVING);
                 Serializer::push_to_server(&m_backward, m_axis_id);
             }
                 break;
@@ -501,6 +501,7 @@ void AxisController::process_message(yat::Message& msg)
             case CTRL_FORWARD_MSG:
             {
                 DEBUG_STREAM << "-> AxisController::TASK_CTRL_FORWARD_MSG" << std::endl;
+                set_state(Tango::MOVING);
                 Serializer::push_to_server(&m_forward, m_axis_id);
             }
                 break;

src/piaxis/PIAxis.cpp

@@ -781,6 +781,7 @@ void PIAxis::forward()
 	
 	try
 	{
+		INFO_STREAM << "PIAxis::forward() - m_model:  " << m_axis_controller->get_model() << std::endl;
 		if(m_axis_controller->get_model() == "C-884" || m_axis_controller->get_model() == "SIMULATOR")
 		{
 			m_axis_controller->forward();

test/config_PIController.txt

@@ -25,16 +25,10 @@ model=(c)MOCK, SIMULATOR, 111111111, 1.0.0.0
 servo=1
 pos=10
 vel=0.500000
-ERR?
-ERR?
-ERR?
 accel=2.000000
 decel=2.000000
 moving=0
 ron=1
-ERR?
-ERR?
-ERR?
 err=0
 limit_switch=0x9004
 is_initialized=1