Skip to content
Snippets Groups Projects
Select Git revision
  • databrowser-split
  • main default protected
  • cea
  • 1.3.13
  • v1.3.12
  • v1.3.11
  • v1.3.10
  • v1.3.9
  • v1.3.8
  • v1.3.7
  • v1.3.6
  • v1.3.1-7920
  • v1.3.0-7615
  • v1.3.0-7521
  • v1.2.0-6800
  • databrowser-1.3.5
  • databrowser-1.3.4
  • databrowser-1.3.3
  • DataBrowser-1.3.2
  • databrowser-1.3.1
  • trunk
  • DataBrowser-before-separation
  • DataBrowser-1.2.1
23 results

pom_cea.xml

Blame
  • 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;
    }