Get the axis init state in the init device

src/piaxis/AxisController.cpp

@@ -7,6 +7,7 @@
 /*************************************************************************/
 
 #include <yat/utils/Logging.h>
+#include <yat/threading/Thread.h>
 
 #include "AxisController.h"
 #include "Serializer.h"
@@ -59,7 +60,7 @@ void AxisController::compute_state_status()
 {
     try
     {
-        if(m_state != Tango::FAULT)
+        if(m_state != Tango::FAULT && m_state != Tango::INIT)
         {
             Serializer::pull_from_server(&m_on, m_axis_id);
             if (m_on.get_value() == 1)
@@ -71,6 +72,7 @@ void AxisController::compute_state_status()
                 {
                     set_state(Tango::MOVING);
                     set_status("Moving");
+                    return;
                 }
             }
             else
@@ -78,15 +80,29 @@ void AxisController::compute_state_status()
                 set_state(Tango::OFF);
                 set_status("Motor off");
             }
+        }
+    }
+    catch(Tango::DevFailed& df)
+    {
+        tango_error_handler(df);
+    }
+    catch(const yat::Exception& e)
+    {
+        yat_error_handler(e,"Exception from - AxisController::compute_state_status() :\n");
+    }
+}
 
-            Serializer::pull_from_server(&m_is_initialized, m_axis_id); //Test
+void AxisController::is_axis_initialized()
+{
+    try
+    {
+        Serializer::pull_from_server(&m_is_initialized, m_axis_id);
         if (m_is_initialized.get_value() == 0)
         {
             set_state(Tango::INIT);
             set_status("Axis need to be initialized");
         }
     }
-    }
     catch(Tango::DevFailed& df)
     {
         tango_error_handler(df);
@@ -311,14 +327,19 @@ void AxisController::auto_zero()
 
 void AxisController::initialise_reference_position()
 {
-    yat::Message* msg = yat::Message::allocate(CTRL_RON_MSG, DEFAULT_MSG_PRIORITY, true);
-    msg->attach_data<bool>(false);
-    post(msg);
     m_velocity.set_value(m_init_velocity);
-    msg = yat::Message::allocate(CTRL_SETVELOCITY_MSG, DEFAULT_MSG_PRIORITY, true);
+    yat::Message* msg = yat::Message::allocate(CTRL_SETVELOCITY_MSG, DEFAULT_MSG_PRIORITY, true);
     post(msg);
-    if (m_model.get_value() == "C-884" || m_model.get_value() == "SIMULATOR")
+    if (m_model.get_value() == "E-754")
     {
+        msg = yat::Message::allocate(CTRL_AUTO_ZERO_MSG, DEFAULT_MSG_PRIORITY, true);
+        post(msg);
+    }
+    else
+    {
+        msg = yat::Message::allocate(CTRL_RON_MSG, DEFAULT_MSG_PRIORITY, true);
+        msg->attach_data<bool>(false);
+        post(msg);
         if (m_init_type == "FNL")
         {
             msg = yat::Message::allocate(CTRL_BACKWARD_MSG, DEFAULT_MSG_PRIORITY, true);
@@ -329,23 +350,14 @@ void AxisController::initialise_reference_position()
             msg = yat::Message::allocate(CTRL_FORWARD_MSG, DEFAULT_MSG_PRIORITY, true);
             post(msg);
         }
-    }
-    else if (m_model.get_value() == "E-754")
-    {
-        yat::Message* msg = yat::Message::allocate(CTRL_AUTO_ZERO_MSG, DEFAULT_MSG_PRIORITY, true);
-        post(msg, 5000); 
-    }
-    while (get_state() == Tango::MOVING)
-    {
-        continue;
-    }
-    m_pos.set_value(m_init_position);
+        /*m_pos.set_value(m_init_position);
         msg = yat::Message::allocate(CTRL_POS_MSG, DEFAULT_MSG_PRIORITY, true);
-    post(msg);
+        post(msg);*/
         msg = yat::Message::allocate(CTRL_RON_MSG, DEFAULT_MSG_PRIORITY, true);
         msg->attach_data<bool>(true);
         post(msg);
     }
+}
 //----------------------------------------------------
 // AxisController::process_message()
 //----------------------------------------------------
@@ -554,6 +566,7 @@ void AxisController::process_message(yat::Message& msg)
             
             case CTRL_AUTO_ZERO_MSG:
             {
+                set_state(Tango::MOVING);
                 Serializer::push_to_server(&m_auto_zero, m_axis_id);
             }
                 break;

src/piaxis/AxisController.h

@@ -68,6 +68,9 @@ public:
     //! \brief Get the Hardware Controller model
     std::string get_model();
 
+    //! \brief Set state and status accordingly
+    void is_axis_initialized();
+    
     //! \brief Turn motor on
     void on();
 

src/piaxis/PIAxis.cpp

@@ -192,6 +192,7 @@ void PIAxis::init_device()
 								axisInitPosition, axisInitVelocity), yat4tango::DeviceTaskExiter());
 		m_axis_controller->go();
 		m_axis_controller->get_model();
+		m_axis_controller->is_axis_initialized();
 	}
 	catch(Tango::DevFailed& df)
 	{

src/pibox/BoxController.cpp

@@ -27,6 +27,7 @@ BoxController::BoxController(yat::String ip_address, int port, yat::StringDictio
     try
     {
         SocketManager::instantiate(ip_address, port, dict, dev);
+        PIBox_ns::SocketManager::get_instance()->write_read("ERR?\n"); // Catch the hardware turning on error
     }
     catch(const yat::Exception& e)
     {