Skip to content
Snippets Groups Projects
Commit db8677b4 authored by DESGRANGES's avatar DESGRANGES
Browse files

[ICATHALES-415] - InitRefPos for the E-754

parent 414ea5d1
Branches
Tags
1 merge request!3[PIController] Add PIAxis commands and attributes
......@@ -60,6 +60,7 @@ void AxisController::compute_state_status()
{
try
{
Serializer::pull_from_server(&m_is_initialized); //Test
if (get_state() == Tango::FAULT)
{
return;
......@@ -290,13 +291,52 @@ bool AxisController::forward_limit()
return (m_limit_switch.get_value() >> 2) & 1;
}
//----------------------------------------------------
// AxisController::auto_zero()
//----------------------------------------------------
void AxisController::auto_zero()
{
yat::Message* msg = yat::Message::allocate(CTRL_AUTO_ZERO_MSG, DEFAULT_MSG_PRIORITY, true);
wait_msg_handled(msg, 5000);
}
//----------------------------------------------------
// AxisController::initialise_reference_position()
//----------------------------------------------------
void AxisController::initialise_reference_position()
{
yat::Message* msg = yat::Message::allocate(CTRL_INIT_REFPOS_MSG, DEFAULT_MSG_PRIORITY, true);
yat::Message* msg = yat::Message::allocate(CTRL_RON_MSG, DEFAULT_MSG_PRIORITY, true);
msg->attach_data(0);
post(msg);
m_velocity.set_value(m_init_velocity);
msg = yat::Message::allocate(CTRL_SETVELOCITY_MSG, DEFAULT_MSG_PRIORITY, true);
post(msg);
if (m_model.get_value() == "C-884")
{
if (m_init_type == "FNL")
{
msg = yat::Message::allocate(CTRL_BACKWARD_MSG, DEFAULT_MSG_PRIORITY, true);
post(msg);
}
else if (m_init_type == "FPL")
{
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);
wait_msg_handled(msg, 5000);
}
m_pos.set_value(m_init_position);
msg = yat::Message::allocate(CTRL_POS_MSG, DEFAULT_MSG_PRIORITY, true);
post(msg);
msg->attach_data(1);
msg = yat::Message::allocate(CTRL_RON_MSG, DEFAULT_MSG_PRIORITY, true);
post(msg);
}
//----------------------------------------------------
......@@ -466,7 +506,7 @@ void AxisController::process_message(yat::Message& msg)
break;
//-----------------------------------------------------
case CTRL_INIT_REFPOS_MSG:
/*case CTRL_INIT_REFPOS_MSG:
{
DEBUG_STREAM << "-> AxisController::CTRL_INIT_REFPOS_MSG" << std::endl;
m_ron.set_value(0);
......@@ -486,6 +526,29 @@ void AxisController::process_message(yat::Message& msg)
m_ron.set_value(1);
Serializer::push_to_server(&m_ron);
}
break;
//-----------------------------------------------------*/
case CTRL_RON_MSG:
{
m_ron.set_value(msg.get_data<int>());
Serializer::push_to_server(&m_ron);
}
break;
//-----------------------------------------------------
case CTRL_POS_MSG:
{
m_pos.set_value(m_init_position);
Serializer::push_to_server(&m_pos);
}
break;
//-----------------------------------------------------
case CTRL_AUTO_ZERO_MSG:
{
Serializer::push_to_server(&m_auto_zero);
}
break;
//-----------------------------------------------------
......
......@@ -43,7 +43,9 @@ const size_t CTRL_FORWARD_MSG = yat::FIRST_USER_MSG + 230;
const size_t CTRL_NLIMIT_MSG = yat::FIRST_USER_MSG + 240;
const size_t CTRL_PLIMIT_MSG = yat::FIRST_USER_MSG + 250;
const size_t CTRL_INIT_REFPOS_MSG = yat::FIRST_USER_MSG + 260;
const size_t CTRL_RON_MSG = yat::FIRST_USER_MSG + 270;
const size_t CTRL_POS_MSG = yat::FIRST_USER_MSG + 280;
const size_t CTRL_AUTO_ZERO_MSG = yat::FIRST_USER_MSG + 290;
namespace PIAxis_ns
{
......@@ -132,8 +134,12 @@ public:
//! \brief initialise the reference position point
void initialise_reference_position();
//! \brief process messages task
void process_message(yat::Message& msg);
//! \brief initialise the reference position point
void auto_zero();
//! \brief error handler for tango
void tango_error_handler(Tango::DevFailed& df);
......@@ -197,6 +203,12 @@ private:
//! \brief m_init_velocity
double m_init_velocity;
//! \brief m_is_initialized
IsInitialized m_is_initialized;
//! \brief m_autozero
AutoZero m_auto_zero;
//! \brief m_state: internal state of PIBox
Tango::DevState m_state;
......
......@@ -576,4 +576,73 @@ void Pos::set_value(double arg)
m_value = arg;
}
// ---------------- IsInitialized ---------------- //
//----------------------------------------------------
// IsInitialized::pull_request()
// Command to know is the device is initiliazed
//----------------------------------------------------
yat::String IsInitialized::pull_request()
{
yat::String return_str;
if (m_model == "C-884" || m_model == "SIMULATOR")
{
return_str = "FRF? " + yat::StringUtil::to_string(m_axis_id);
}
else if (m_model == "E-754")
{
return_str = "ATZ? " + yat::StringUtil::to_string(m_axis_id);
}
else
{
Tango::Except::throw_exception("INTERNAL_ERROR",
"Command not handled for this device",
"Backward::push_request()");
}
return return_str;
}
//----------------------------------------------------
// IsInitialized::extract()
// Extract the information from the response to the
// pull request
//----------------------------------------------------
void IsInitialized::extract(yat::String str)
{
size_t pos = str.find("=");
if (pos != std::string::npos)
{
m_value = yat::StringUtil::to_num<int>(str.substr(pos + 1, str.size() - 2));
}
}
//----------------------------------------------------
// IsInitialized::push_request()
//----------------------------------------------------
yat::String IsInitialized::push_request()
{
Tango::Except::throw_exception("INTERNAL_ERROR",
"This method is not available",
"IsInitialized::push_request()");
return " ";
}
//----------------------------------------------------
// IsInitialized::get_value()
//----------------------------------------------------
bool IsInitialized::get_value()
{
return m_value;
}
// ---------------- AutoZero ---------------- //
//----------------------------------------------------
// AutoZero::push_request()
// Command to move to ZERO
//----------------------------------------------------
yat::String AutoZero::push_request()
{
return "ATZ " + yat::StringUtil::to_string(m_axis_id) + " NaN";
}
} // namespace PIAxis_ns
......@@ -213,6 +213,24 @@ private:
int m_value;
};
class IsInitialized : public Serializable
{
public:
yat::String pull_request();
void extract(yat::String str);
yat::String push_request();
bool get_value();
private:
bool m_value;
};
class AutoZero : public Command
{
public:
yat::String push_request();
};
} // namespace PIAxis_ns
......
......@@ -179,10 +179,10 @@ void BoxController::process_message(yat::Message& msg)
}
else
{
cmd = 5 + "\n";
cmd = 5;
DEBUG_STREAM << "BoxController::process_message::EXEC_LOW_LEVEL_CMD_MSG - NEW CMD: " << cmd << std::endl;
}
m_exec_low_level_cmd_out_value = PIBox_ns::SocketManager::get_instance()->write_read(cmd).c_str();
m_exec_low_level_cmd_out_value = PIBox_ns::SocketManager::get_instance()->write_read(cmd + "\n").c_str();
}
else
{
......
......@@ -277,8 +277,20 @@ yat::String SocketManager::read()
//----------------------------------------------------
yat::String SocketManager::write_read(yat::String buffer)
{
write(buffer);
return read();
yat::MutexLock scoped_lock(m_locker);
yat::Socket::Data w_data(256, true);
w_data.memcpy(buffer.c_str(), buffer.size());
m_socket << w_data;
status_handler();
m_socket >> w_data;
status_handler();
int size = w_data.size();
yat::String response = w_data.base();
return response;
}
//----------------------------------------------------
......
......@@ -23,7 +23,9 @@
#this i a special section to declare user variables, the syntaxe is xxx=yyy
model=(c)MOCK, SIMULATOR, 111111111, 1.0.0.0
servo=1
pos=8.000000
ERR?
ERR?
pos=2.000000
vel=0.500000
accel=1.000000
decel=1.000000
......@@ -31,6 +33,7 @@ moving=0
ron=1
err=0
limit_switch=0x9002
is_initialized=1
[END]
[*IDN?]
......@@ -160,3 +163,9 @@ my_var = globals()['read_vars']('limit_switch')
reply = command.split(" ")[1] + str(" 1=") + str(my_var)
verbose = True
[END]
[FRF?]
my_var = globals()['read_vars']('is_initialized')
reply = command.split(" ")[1] + str("=") + str(my_var)
verbose = True
[END]
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment