Select Git revision
AxisSimulatorThread.cpp
Code owners
Assign users and groups as approvers for specific file changes. Learn more.
AxisSimulatorThread.cpp 10.46 KiB
#include <AxisSimulatorThread.h>
#include <math.h>
#include <TangoExceptionsHelper.h>
/*********************************************************************
* AxisSimulatorThread::AxisSimulatorThread
**********************************************************************/
AxisSimulatorThread::AxisSimulatorThread(Tango::DeviceImpl* _dev) :
Tango::Thread(),
Tango::LogAdapter(_dev)
{
// for the device to wait run_undetached is started
////this->oc = c;
// not used
this->req = REQ_STOP;
this->dev_ = _dev;
DEBUG_STREAM <<"AxisSimulatorThread::AxisSimulatorThread <-<-<-"<<endl;
this->relative_move = 0.0;
this->actual_move = MOVE_STOP;
this->health_state = HEALTH_OK;
this->polling_period_nsec = 1000000000;
this->polling_period_sec = 0;
this->limit_low_active = false;
this->limit_high_active = false;
this->positionned = false;
DEBUG_STREAM <<"AxisSimulatorThread::AxisSimulatorThread ->->->"<<endl;
}
/*********************************************************************
* AxisSimulatorThread::~AxisSimulatorThread
**********************************************************************/
AxisSimulatorThread::~AxisSimulatorThread(void)
{
DEBUG_STREAM <<"AxisSimulatorThread::~AxisSimulatorThread <-<-<-"<<endl;
}
/*********************************************************************
* AxisSimulatorThread::stop
**********************************************************************/
void AxisSimulatorThread::stop(void)
{
DEBUG_STREAM <<"AxisSimulatorThread::stop <-<-<-"<<endl;
this->req = REQ_STOP;
// void** arg = 0;
// this->join(arg);
}
/*********************************************************************
* AxisSimulatorThread::exit
**********************************************************************/
void AxisSimulatorThread::exit(void)
{
DEBUG_STREAM <<"AxisSimulatorThread::exit <-<-<-"<<endl;
this->req = REQ_EXIT;
// void** arg = 0;
// this->join(arg);
}
/*********************************************************************
* AxisSimulatorThread::fwd
**********************************************************************/
void AxisSimulatorThread::fwd(void)
{
DEBUG_STREAM << "AxisSimulatorThread::fwd <-<-<-" <<endl;
INFO_STREAM <<"speed : " << this->speed << endl;
INFO_STREAM <<"step size : " << this->step << endl;
if(!this->limit_high_active)
this->req = REQ_FORWARD;
this->positionned = false;
this->calc_polling_period();
}
/*********************************************************************
* AxisSimulatorThread::bkwd
**********************************************************************/
void AxisSimulatorThread::bkwd(void)
{
DEBUG_STREAM <<"AxisSimulatorThread::bkwd <-<-<-" <<endl;
INFO_STREAM <<"speed : " << this->speed << endl;
INFO_STREAM <<"step size : " << this->step << endl;
if(!this->limit_low_active)
this->req = REQ_BACKWARD;
positionned = false;
this->calc_polling_period();
}
/*********************************************************************
* AxisSimulatorThread::simufault
**********************************************************************/
void AxisSimulatorThread::simufault(int)
{
DEBUG_STREAM << "AxisSimulatorThread::simufault <-<-<-"<<endl;
this->req = REQ_FAULT;
}
/*********************************************************************
* AxisSimulatorThread::gotopos
**********************************************************************/
void AxisSimulatorThread::gotopos(double d)
{
DEBUG_STREAM << "AxisSimulatorThread::gotopos <-<-<-"<<endl;
INFO_STREAM <<"speed : " << this->speed << endl;
INFO_STREAM <<"step size : " << this->step << endl;
this->relative_move = d - this->pos;
this->setpoint = d;
if(this->relative_move >=0)
this->req = REQ_GOTOPOS_FWD;
else
this->req = REQ_GOTOPOS_BKWD;
this->positionned = false;
this->calc_polling_period();
}
/*********************************************************************
* AxisSimulatorThread::set_speed
**********************************************************************/
void AxisSimulatorThread::set_speed(double speed)
{
DEBUG_STREAM << "AxisSimulatorThread::set_speed <-<-<-"<<endl;
this->speed = speed;
this->calc_polling_period();
}
/*********************************************************************
* AxisSimulatorThread::calc_polling_period
used to simulate the motor speed
**********************************************************************/
void AxisSimulatorThread::calc_polling_period(void)
{
if(this->speed < static_cast<double>(0.001))
this->speed = static_cast<double>(0.001);
double tmp = static_cast<double>((1000000000.0)/this->speed);
this->polling_period_nsec = (unsigned long)tmp;
INFO_STREAM<<"polling period (ms) : " << (double)((double)this->polling_period_nsec/1000000.0)<< endl;
}
/*********************************************************************
* SequenceThread::run_undetached
**********************************************************************/
void* AxisSimulatorThread::run_undetached(void* arg)
{
DEBUG_STREAM << "AxisSimulatorThread::run_undetached <-<-<-"<<endl;
// signals to the device server that thread is started
this->actual_move = MOVE_STOP;
if(this->accuracy <=0)
this->accuracy = 1.0;
if(this->speed <= static_cast<double>(0.0))
this->speed = static_cast<double>(1.0);
DEBUG_STREAM<<"************************* BEGIN THREAD LOOP ***************************"<<endl;
do
{
// some variables to initialise
// polling period to simulate speed
// -------------------- SOFTWARE LIMITS SIMULATION --------------------
// look the software limits if configured
// ( via min and max _value properties of attribute Position ):
if((this->software_limit_low_valid) && (this->pos <= this->software_limit_low))
{
DEBUG_STREAM << "LIMIT SWITCH BACKWARD ACTIVE" << endl;
this->limit_low_active = true;
}
else
{
this->limit_low_active = false;
}
if((this->software_limit_high_valid) && (this->pos >= this->software_limit_high))
{
DEBUG_STREAM << "LIMIT SWITCH FORWARD ACTIVE" << endl;
this->limit_high_active = true;
}
else
{
this->limit_high_active = false;
}
// -------------------- STATE MACHINE --------------------
// STOP ACTIVITY
if (((this->actual_move == MOVE_FORWARD) && (this->req == REQ_STOP)) ||
((this->actual_move == MOVE_BACKWARD) && (this->req == REQ_STOP)) ||
((this-> actual_move == POS_OK) && (this->req == REQ_FORWARD)) ||
((this-> actual_move == POS_OK) && (this->req == REQ_GOTOPOS_FWD) && !this->positionned) ||
((this-> actual_move == POS_OK) && (this->req == REQ_BACKWARD)) ||
((this-> actual_move == POS_OK) && (this->req == REQ_GOTOPOS_BKWD) && !this->positionned) ||
((this-> actual_move == POS_OK) && (this->req == REQ_STOP)) ||
((this-> actual_move == POS_NOK) && (this->req == REQ_FORWARD)) ||
((this-> actual_move == POS_NOK) && (this->req == REQ_GOTOPOS_FWD) && !this->positionned) ||
((this-> actual_move == POS_NOK) && (this->req == REQ_BACKWARD)) ||
((this-> actual_move == POS_NOK) && (this->req == REQ_GOTOPOS_BKWD) && !this->positionned) ||
((this-> actual_move == POS_NOK) && (this->req == REQ_STOP)) )
{
this->actual_move = MOVE_STOP;
DEBUG_STREAM << "actual_move = MOVE_STOP" << endl;
}
// FORWARD ACTIVITY
if( ((this->actual_move == MOVE_STOP) && (this->req == REQ_FORWARD)) ||
((this->actual_move == MOVE_STOP) && (this->req == REQ_GOTOPOS_FWD) && !this->positionned) ||
((this->actual_move == STOPPED_LSBKWD) && (this->req == REQ_FORWARD)) ||
((this->actual_move == STOPPED_LSBKWD) && (this->req == REQ_GOTOPOS_FWD) && !this->positionned) )
{
this->actual_move = MOVE_FORWARD;
DEBUG_STREAM << "actual_move = MOVE_FORWARD" << endl;
}
// BACKWARD ACTIVITY
if( ((this->actual_move == MOVE_STOP) && (this->req == REQ_BACKWARD)) ||
((this->actual_move == MOVE_STOP) && (this->req == REQ_GOTOPOS_BKWD) && !this->positionned) ||
((this->actual_move == STOPPED_LSFWD) && (this->req == REQ_BACKWARD)) ||
((this->actual_move == STOPPED_LSFWD) && (this->req == REQ_GOTOPOS_BKWD) && !this->positionned) )
{
this->actual_move = MOVE_BACKWARD;
DEBUG_STREAM << "actual_move = MOVE_BACKWARD" << endl;
}
// ON LIMIT SWITCH FORWARD ACTIVITY
if( (this->actual_move == MOVE_FORWARD) && this->limit_high_active )
{
this->actual_move = STOPPED_LSFWD;
DEBUG_STREAM << "actual_move = STOPPED_LSFWD" << endl;
}
// ON LIMIT SWITCH BACKWARD ACTIVITY
if( (this->actual_move == MOVE_BACKWARD) && this->limit_low_active )
{
this->actual_move = STOPPED_LSBKWD;
DEBUG_STREAM << "actual_move = STOPPED_LSBKWD" << endl;
}
// POS_OK activity : position OK or NOT OK
if( ((this->actual_move == MOVE_FORWARD)||(this->actual_move == MOVE_BACKWARD)) && this->positionned )
{
double delta = fabs((this->pos - this->setpoint));
DEBUG_STREAM << "pos : " << pos << " setpoint : " << this->setpoint << " delta : " << delta << " accuracy : " << this->accuracy << endl;
if( delta <= this->accuracy )
{
this->actual_move = POS_OK;
DEBUG_STREAM << "actual_move = POS_OK" << endl;
}
else
{
this->actual_move = POS_NOK;
DEBUG_STREAM << "actual_move = POS_NOT_OK !!!!" << endl;
}
}
// -------------------- SIMULATE MOVE --------------------
if( (this->actual_move == MOVE_FORWARD) && !this->limit_high_active ) // si forward on incremente
{
this->pos += step;
DEBUG_STREAM<<"position : "<<this->pos<<endl;
// std::cout << "+" ;
}
if( (this->actual_move == MOVE_BACKWARD) && !this->limit_low_active )// si backward on decremente
{
this->pos -= step;
DEBUG_STREAM<<"position : "<<this->pos<<endl;
// std::cout << "-" ;
}
// -------------------- SIMULATE POSITION OK --------------------
if(!this->positionned && (this->req == REQ_GOTOPOS_BKWD) )
{
if(this->pos <= this->setpoint)
{
this->positionned = true;
}
}
if(!this->positionned && (this->req == REQ_GOTOPOS_FWD))
{
if(this->pos >= this->setpoint)
{
this->positionned = true;
}
}
// -------------------- POLLING PERIOD slow down if no move --------------------
if( (this->actual_move != MOVE_FORWARD) && (this->actual_move != MOVE_BACKWARD) )
{
// stopped : polling period of O,1 s
this->polling_period_nsec = 1000000000;
}
// endormir le thread pour le temps
this->sleep(this->polling_period_sec, this->polling_period_nsec);
}while(this->req != REQ_EXIT);
DEBUG_STREAM<<"************************* END THREAD LOOP ***************************"<<endl;
return 0;
}