Simplify the state machine in compute_state().

src/AxisSimulatorTask.cpp

@@ -42,7 +42,7 @@ AxisSimulatorTask::AxisSimulatorTask(size_t _periodic_timeout_ms, Tango::DeviceI
 	this->_fAccuracy 				= kDefaultAccuracy;
 	this->_fCurrentPosition 		= 0.0;
 	this->_fNewPosition 			= 0.0;
-	this->_tRequest 				= REQ_STOP;	
+	this->_tRequest 				= REQ_NONE;	
 	this->_tCurrentMove 			= MOVE_STOP;
 	this->_bSoftwareLimitLowValid 	= false;
 	this->_bLimitLowActive 			= false;
@@ -50,7 +50,6 @@ AxisSimulatorTask::AxisSimulatorTask(size_t _periodic_timeout_ms, Tango::DeviceI
 	this->_bSoftwareLimitHighValid 	= false;	
 	this->_bLimitHighActive 		= false;	
 	this->_fSoftwareLimitHigh 		= kDefaultSoftwareLimitHigh;
-	this->_bPositionned 			= false;    
 	this->_dsState 					= Tango::INIT;
     this->_sStatusMessage 			= "";
 	
@@ -125,9 +124,12 @@ void AxisSimulatorTask::process_message (yat::Message& _msg)
 					  this->sleep(1);//hyper precision sleep(1)->really ~1 ms : this is due to timeBeginPeriod()/timeEndPeriod()
 				  }
 				  DEBUG_STREAM<<"Time elapsed = "<<static_cast<unsigned long>(this->_mChrono.elapsed_msec()-this->_fLastChronoValue)<<" (ms)"<<endl;
+				  
+				  {
 					yat::AutoMutex<> guard(this->m_lock);   
 					this->_fLastChronoValue = this->_mChrono.elapsed_msec();				
 					this->compute_state();
+				  }
   
 			  }
 			  catch(const Tango::DevFailed& df)
@@ -222,10 +224,9 @@ void AxisSimulatorTask::stop(void)
 // ============================================================================
 void AxisSimulatorTask::forward(void)
 {   
-	DEBUG_STREAM << "AxisSimulatorTask::forward <-<-<-"	<<endl;
+	INFO_STREAM << "AxisSimulatorTask::forward <-<-<-"	<<endl;
 	if(!this->_bLimitHighActive)
 		this->_tRequest = REQ_FORWARD;
-	this->_bPositionned = false;	
 }
 
 // ============================================================================
@@ -233,10 +234,9 @@ void AxisSimulatorTask::forward(void)
 // ============================================================================
 void AxisSimulatorTask::backward(void)
 {   
-	DEBUG_STREAM <<"AxisSimulatorTask::backward <-<-<-"	<<endl;
+	INFO_STREAM <<"AxisSimulatorTask::backward <-<-<-"	<<endl;
 	if(!this->_bLimitLowActive)
 		this->_tRequest = REQ_BACKWARD;
-	_bPositionned = false;
 }
 
 
@@ -259,7 +259,6 @@ void AxisSimulatorTask::set_position(double fPosition)
 		this->_tRequest = REQ_GOTOPOS_BKWD;
 		
 	this->_fNewPosition = fPosition;
-	this->_bPositionned = false;	
 }
 
 // ============================================================================
@@ -387,11 +386,10 @@ void AxisSimulatorTask::compute_state (void)
 {    
    DEBUG_STREAM << "AxisSimulatorTask::compute_state <-<-<-"<<endl;
 
-	this->_tCurrentMove = MOVE_STOP;
-		
 	// ------------------------------------------------------------------------
 	//							SOFTWARE LIMITS SIMULATION
 	// ------------------------------------------------------------------------
+	
 	// look the software limits if configured
 	// this is done via min and max _value properties of attribute Position
 	if((this->_bSoftwareLimitLowValid) && (this->_fCurrentPosition <= this->_fSoftwareLimitLow))
@@ -414,117 +412,121 @@ void AxisSimulatorTask::compute_state (void)
 		this->_bLimitHighActive = false;
 	}
 
+
 	// ------------------------------------------------------------------------	
-	//				STATE MACHINE (must be simplified : very complicated)
+	//								STATE MACHINE
 	// ------------------------------------------------------------------------
 	
-	// ------------------------------------------------------------------------
-	// STOP ACTIVITY
-	if (((this-> _tCurrentMove == MOVE_FORWARD)	&& (this->_tRequest == REQ_STOP))				 					||
-		((this-> _tCurrentMove == MOVE_BACKWARD)&& (this->_tRequest == REQ_STOP))				 					||
-		((this-> _tCurrentMove == POS_OK)		&& (this->_tRequest == REQ_FORWARD)) 		 						||
-		((this-> _tCurrentMove == POS_OK)		&& (this->_tRequest == REQ_GOTOPOS_FWD)	&& !this->_bPositionned)	||
-		((this-> _tCurrentMove == POS_OK)		&& (this->_tRequest == REQ_BACKWARD))		 						||
-		((this-> _tCurrentMove == POS_OK)		&& (this->_tRequest == REQ_GOTOPOS_BKWD) && !this->_bPositionned) 	||
-		((this-> _tCurrentMove == POS_OK)		&& (this->_tRequest == REQ_STOP))				 					||
-		((this-> _tCurrentMove == POS_NOK)		&& (this->_tRequest == REQ_FORWARD))		 						||
-		((this-> _tCurrentMove == POS_NOK) 		&& (this->_tRequest == REQ_GOTOPOS_FWD)  && !this->_bPositionned)	||
-		((this-> _tCurrentMove == POS_NOK) 		&& (this->_tRequest == REQ_BACKWARD)) 	 							||
-		((this-> _tCurrentMove == POS_NOK) 		&& (this->_tRequest == REQ_GOTOPOS_BKWD) && !this->_bPositionned)	||
-		((this-> _tCurrentMove == POS_NOK) 		&& (this->_tRequest == REQ_STOP)) )
+	// REQ_STOP ACTIVITY
+	if (this->_tRequest == REQ_STOP)	
 	{
 		this->_tCurrentMove = MOVE_STOP;
-		DEBUG_STREAM << "actual_move = MOVE_STOP" << endl;
-
+		DEBUG_STREAM << "current_move = MOVE_STOP" << endl;								
 	}
 	
-	// ------------------------------------------------------------------------
-	// FORWARD ACTIVITY
-	if( ((this->_tCurrentMove == MOVE_STOP)			&& (this->_tRequest == REQ_FORWARD))  							||
-		((this->_tCurrentMove == MOVE_STOP)			&& (this->_tRequest == REQ_GOTOPOS_FWD) && !this->_bPositionned)||
-		((this->_tCurrentMove == STOPPED_LSBKWD)	&& (this->_tRequest == REQ_FORWARD))  							||
-		((this->_tCurrentMove == STOPPED_LSBKWD)	&& (this->_tRequest == REQ_GOTOPOS_FWD) && !this->_bPositionned)  )
-	{
-		this->_tCurrentMove = MOVE_FORWARD;
-		DEBUG_STREAM << "actual_move = MOVE_FORWARD" << endl;
-	}
 		
-	// ------------------------------------------------------------------------
-	// BACKWARD ACTIVITY
-	if( ((this->_tCurrentMove == MOVE_STOP)		&& (this->_tRequest == REQ_BACKWARD))								||
-		((this->_tCurrentMove == MOVE_STOP)		&& (this->_tRequest == REQ_GOTOPOS_BKWD) && !this->_bPositionned) 	||
-		((this->_tCurrentMove == STOPPED_LSFWD)	&& (this->_tRequest == REQ_BACKWARD))								||
-		((this->_tCurrentMove == STOPPED_LSFWD)	&& (this->_tRequest == REQ_GOTOPOS_BKWD) && !this->_bPositionned)	)
+	// REQ_GOTOPOS_FWD ACTIVITY || REQ_FORWARD ACTIVITY
+	if (this->_tRequest == REQ_GOTOPOS_FWD || this->_tRequest == REQ_FORWARD)	
 	{	
-		this->_tCurrentMove = MOVE_BACKWARD;
-		DEBUG_STREAM << "actual_move = MOVE_BACKWARD" << endl;
-	}
-		
-	// ------------------------------------------------------------------------		
-	// ON LIMIT SWITCH FORWARD ACTIVITY
-	if( (this->_tCurrentMove == MOVE_FORWARD) && this->_bLimitHighActive )
+		if(this->_bLimitHighActive)	//limit switch reached
 		{
 			this->_tCurrentMove = STOPPED_LSFWD;
-		DEBUG_STREAM << "actual_move = STOPPED_LSFWD" << endl;
+			this->_tRequest = REQ_NONE;
+			INFO_STREAM << "current_move = STOPPED_LSFWD" << endl;	
 		}
-		
-	// ------------------------------------------------------------------------		
-	// ON LIMIT SWITCH BACKWARD ACTIVITY
-	if( (this->_tCurrentMove == MOVE_BACKWARD) && this->_bLimitLowActive )
+		else
 		{
-		this->_tCurrentMove = STOPPED_LSBKWD;
-		DEBUG_STREAM << "actual_move = STOPPED_LSBKWD" << endl;
+			if(this->_tRequest == REQ_FORWARD)	//	always go forward  
+			{
+				this->_fCurrentPosition += this->_fStep;
+				INFO_STREAM<<"position = "<< this->_fCurrentPosition<<endl;
+				this->_tCurrentMove = MOVE_FORWARD;
+				DEBUG_STREAM << "current_move = MOVE_FORWARD" << endl;						
 			}
-
-	// ------------------------------------------------------------------------	
-	// POS_OK activity : position OK or NOT OK
-	if( ((this->_tRequest == REQ_GOTOPOS_BKWD)||(this->_tRequest == REQ_GOTOPOS_FWD)) && this->_bPositionned )
+			else
 			{
 				double delta = fabs((this->_fCurrentPosition - this->_fNewPosition));
 				DEBUG_STREAM<<"delta = "<< delta<<endl;			
-		if( delta <= (this->_fAccuracy-EPSILON) )
+				if( delta > (this->_fStep) )	//not near requested position
+				{
+					this->_fCurrentPosition += this->_fStep;
+					INFO_STREAM<<"position = "<< this->_fCurrentPosition<<endl;					
+					this->_tCurrentMove = MOVE_FORWARD;
+					DEBUG_STREAM << "current_move = MOVE_FORWARD" << endl;							
+				}
+				else
+				{
+					if( delta <= (this->_fAccuracy+EPSILON) )	// determine if the position is accurate enough
 					{
 						this->_tCurrentMove = POS_OK;
-			DEBUG_STREAM << "actual_move = POS_OK" << endl;
+						this->_tRequest = REQ_NONE;
+						INFO_STREAM << "current_move = POS_OK" << endl;
 					}
 					else
 					{
 						this->_tCurrentMove = POS_NOK;
-			DEBUG_STREAM << "actual_move = POS_NOT_OK !!!!" << endl;
+						this->_tRequest = REQ_NONE;
+						INFO_STREAM << "current_move = POS_NOT_OK !!!!" << endl;
+					}
+				}
+			}
 		}
 	}	
 	
-	// ------------------------------------------------------------------------
-	// INCREMENT POSITION WITH THE STEP SIZE
-	if( (this->_tCurrentMove == MOVE_FORWARD) && !this->_bLimitHighActive )  // si forward on incremente
+		
+	// REQ_GOTOPOS_BKWD ACTIVITY || REQ_BACKWARD ACTIVITY
+	if (this->_tRequest == REQ_GOTOPOS_BKWD || this->_tRequest == REQ_BACKWARD)	
 	{		
-		this->_fCurrentPosition += _fStep;
-		INFO_STREAM<<"position = "<< this->_fCurrentPosition<<endl;		
+		if(this->_bLimitLowActive)	//limit switch reached
+		{
+			this->_tCurrentMove = STOPPED_LSBKWD;
+			this->_tRequest = REQ_NONE;
+			INFO_STREAM << "current_move = STOPPED_LSBKWD" << endl;		
 		}
-	if( (this->_tCurrentMove == MOVE_BACKWARD) && !this->_bLimitLowActive )// si backward on decremente
+		else
 		{
-		this->_fCurrentPosition -= _fStep;
+			if(this->_tRequest == REQ_BACKWARD)	//	always go backward  
+			{
+				this->_fCurrentPosition -= this->_fStep;
 				INFO_STREAM<<"position = "<< this->_fCurrentPosition<<endl;
+				this->_tCurrentMove = MOVE_BACKWARD;
+				DEBUG_STREAM << "current_move = MOVE_BACKWARD" << endl;					
 			}
-	
-	// ------------------------------------------------------------------------
-	// CHECK IF NEW POSITION IS REACHED ACCORDING TO ACCURACY
-	if((!this->_bPositionned && (this->_tRequest == REQ_GOTOPOS_BKWD)) || (!this->_bPositionned && (this->_tRequest == REQ_GOTOPOS_FWD)))
+			else
 			{
+				
 				double delta = fabs((this->_fCurrentPosition - this->_fNewPosition));
 				DEBUG_STREAM<<"delta = "<< delta<<endl;			
-		if( delta <= (this->_fAccuracy-EPSILON) )
+				if( delta > (this->_fStep) )	//not near requested position
 				{
-			this->_bPositionned=true;
+					this->_fCurrentPosition -= this->_fStep;					
+					INFO_STREAM<<"position = "<< this->_fCurrentPosition<<endl;					
+					this->_tCurrentMove = MOVE_BACKWARD;
+					DEBUG_STREAM << "current_move = MOVE_BACKWARD" << endl;	
 				}
 				else
 				{
-			this->_bPositionned=false;
+					if( delta <= (this->_fAccuracy+EPSILON) )	// determine if the position is accurate enough
+					{
+						this->_tCurrentMove = POS_OK;
+						this->_tRequest = REQ_NONE;
+						INFO_STREAM << "current_move = POS_OK" << endl;
+					}
+					else
+					{
+						this->_tCurrentMove = POS_NOK;
+						this->_tRequest = REQ_NONE;
+						INFO_STREAM << "current_move = POS_NOT_OK !!!!" << endl;
+					}
+				}
+			}
 		}
 	}
 	
 	// ------------------------------------------------------------------------		
 	// 				update state & status according to actual_move
+	// ------------------------------------------------------------------------
+	
 	switch(this->_tCurrentMove)
 	{
 		case MOVE_STOP :

src/AxisSimulatorTask.h

@@ -38,11 +38,11 @@
 
 #define kDefaultSoftwareLimitLow	-1000
 #define kDefaultSoftwareLimitHigh	1000
-#define kDefaultAccuracy		 	0.01
-#define kMinAccuracy			 	0.001
+#define kDefaultAccuracy		 	0.02
+#define kMinAccuracy			 	0.002
 #define kDefaultAccel			 	1.0
 #define kDefaultDecel			 	1.0
-#define kDefaultStep			 	1.0
+#define kDefaultStep			 	0.01
 #define kMinStep			 		0.001
 
 
@@ -74,8 +74,22 @@
 #define kSETPOSITION_MSG		(yat::FIRST_USER_MSG + 1003)
 #define kSETSPEED_MSG			(yat::FIRST_USER_MSG + 1004)
 
-enum MOVING_STATE {MOVE_STOP,MOVE_FORWARD,MOVE_BACKWARD,STOPPED_LSFWD,STOPPED_LSBKWD,POS_OK,POS_NOK};
-enum REQUEST {REQ_EXIT=-1,REQ_STOP,REQ_FORWARD,REQ_BACKWARD,REQ_GOTOPOS_FWD,REQ_GOTOPOS_BKWD,REQ_FAULT};
+enum MOVING_STATE {	MOVE_NONE=-1,
+					MOVE_STOP,
+					MOVE_FORWARD,
+					MOVE_BACKWARD,
+					STOPPED_LSFWD,
+					STOPPED_LSBKWD,
+					POS_OK,
+					POS_NOK};
+					
+enum REQUEST {	REQ_NONE=-1,
+				REQ_STOP,
+				REQ_FORWARD,
+				REQ_BACKWARD,
+				REQ_GOTOPOS_FWD,
+				REQ_GOTOPOS_BKWD};
+				
 namespace SimulatedMotor_ns
 {
 // ============================================================================
@@ -85,48 +99,48 @@ class AxisSimulatorTask : public yat4tango::DeviceTask
 {
 public:
 
-    //- ctor ---------------------------------
+    //- ctor
     AxisSimulatorTask(size_t _periodic_timeout_ms, Tango::DeviceImpl * _host_device);
     
-    //- dtor ---------------------------------
+    //- dtor
     virtual ~AxisSimulatorTask (void);
 	
-	//- Execute <forward> command, increments position attribute
+	//- Execute <forward> command, increments position attribute.
 	void forward(void);
-	//- Execute <bakward> command, decrements position attribute
+	//- Execute <bakward> command, decrements position attribute.
 	void backward(void);
-	//- Execute <stop> command, stops any movement on position
+	//- Execute <stop> command, stops any movement on position.
 	void stop(void);
-	//- Execute <go_to_position> command, increment position to reach required position value.
+	//- Define position to reach and determine type of movement (FORWARD, BACKWARD, GOTOPOS_FORWARD,GOTOPOS_BACKWARD).
 	void set_position(double);
-	//- Execute <go_to_position> command, increment position to reach required position value.
+	//- Get the current position.
 	double get_position(void);	
-	//- Execute <set_speed> command, fix the speed value.	
+	//- Set the speed value.
 	void set_speed(double);
-	//- Execute <exit> command.
+	//- Get the speed value.
 	double get_speed(void);
-	//- Execute <set_acceleration> command, fix the acceleration value. (fake: no effect on motor)	
+	//- Set the acceleration value. (fake: no effect on motor)	
 	void set_acceleration(double);
-	//- Execute <get_acceleration> command.
+	//- Get the acceleration value. (fake: always = 1)	
 	double get_acceleration(void);
-	//- Execute <set_deceleration> command, fix the deceleration value. (fake: no effect on motor)		
+	//- Set the acceleration value. (fake: no effect on motor)	
 	void set_deceleration(double);
-	//- Execute <get_deceleration> command.
+	//- Get the acceleration value. (fake: always = 1)	
 	double get_deceleration(void);
-	//- Execute <set_accuracy> command, fix the accuracy value.	
+	//- Set the accuracy value.	
 	void set_accuracy(double);
-	//- Execute <get_accuracy> command.
+	//- Get the accuracy value.	
 	double get_accuracy(void);
-	//- Execute <set_step> command, fix the motor step value.	
+	//- Set the motor step value.	
 	void set_step(double);
-	//- Execute <get_step> command.
+	//- Get the motor step value.	
 	double get_step(void);
+	
     //- Get the last computed state of the Motor
     Tango::DevState get_state (void);
     //- Get the last computed status of the Motor
     Tango::DevString get_status (void);
 	
-	
 	// software limit switchs (must be private : lets them in public section for the moment)
 	bool 			_bLimitLowActive;	
 	double			_fSoftwareLimitLow;
@@ -147,9 +161,10 @@ private:
     //- Compute the Motor state : it depends on pending requests (MOVE_FORWARD, MOVE_BACKWARD,....) 
     void compute_state (void);
 	
-    //- Computed state of the Motor
+    // Computed state of the Motor
     Tango::DevState _dsState;
-    //- Computed status of the Motor
+	
+    // Computed status of the Motor
     Tango::DevString _sStatusMessage;		
 
 	// motor internal values
@@ -166,9 +181,6 @@ private:
 	// current user request
 	REQUEST 		_tRequest;	
 	
-	// position OK flag for set_position
-	bool 			_bPositionned;
-	
 	// New position required from set_position user command.
 	double 			_fNewPosition;