예제 #1
0
returnValue Controller::feedbackStep(	double currentTime,
										const Vector& _y,
										const VariablesGrid& _yRef
										)
{
	realClock.reset( );
	
	if ( controlLaw == 0 )
		return ACADOERROR( RET_NO_CONTROLLAW_SPECIFIED );

	/* Do nothing if controller is disabled */
	if ( isEnabled == BT_FALSE )
		return SUCCESSFUL_RETURN;

	// start real runtime measurement
	realClock.start( );
	
	Vector xEst, pEst;

	/* 1) Call Estimator */
	if ( obtainEstimates( currentTime,_y,xEst,pEst ) != SUCCESSFUL_RETURN )
		return ACADOERROR( RET_CONTROLLER_STEP_FAILED );

	/* 2) Evaluate reference trajectory */
	VariablesGrid yRef( _yRef );
	getCurrentReference( currentTime,yRef );
	#ifdef SIM_DEBUG
	yRef.print( "yRef feedback" );
	#endif

	controlLawClock.reset();
	controlLawClock.start();
	
	/* 3) Perform feedback step of control law */
	if ( controlLaw->feedbackStep( currentTime,xEst,pEst,yRef ) != SUCCESSFUL_RETURN )
		return ACADOERROR( RET_CONTROLLER_STEP_FAILED );

	controlLawClock.stop();
	realClock.stop( );

	#ifdef SIM_DEBUG
	Vector uTmp;
	getU( uTmp );
	uTmp.print("u(0) after feedbackStep");
	#endif

	return SUCCESSFUL_RETURN;
}
예제 #2
0
파일: controller.cpp 프로젝트: rtkg/acado
returnValue Controller::feedbackStep(	double currentTime,
										const Vector& _y
										)
{
	if ( controlLaw == 0 )
		return ACADOERROR( RET_NO_CONTROLLAW_SPECIFIED );

	Vector xEst, pEst;

	/* 1) Call Estimator */
	if ( obtainEstimates( currentTime,_y,xEst,pEst ) != SUCCESSFUL_RETURN )
		return ACADOERROR( RET_CONTROLLER_STEP_FAILED );

	/* 2) Perform feedback step of control law */
	if ( controlLaw->feedbackStep( currentTime,xEst,pEst ) != SUCCESSFUL_RETURN )
		return ACADOERROR( RET_CONTROLLER_STEP_FAILED );

	return SUCCESSFUL_RETURN;
}
예제 #3
0
파일: controller.cpp 프로젝트: rtkg/acado
returnValue Controller::step(	double currentTime,
								const Vector& _y,
								const VariablesGrid& _yRef
								)
{
	/* Consistency check. */
	if ( getStatus( ) != BS_READY )
		return ACADOERROR( RET_BLOCK_NOT_READY );

	if ( controlLaw == 0 )
		return ACADOERROR( RET_NO_CONTROLLAW_SPECIFIED );

	// start real runtime measurement
	realClock.reset( );
	realClock.start( );
	
	RealClock clock;

	//printf( "RTI? %d!!\n", controlLaw->isInRealTimeMode( ) );

	if ( controlLaw->isInRealTimeMode( ) == BT_TRUE )
	{
		clock.reset();
		clock.start();

		if ( feedbackStep( currentTime,_y ) != SUCCESSFUL_RETURN )
			return ACADOERROR( RET_CONTROLLER_STEP_FAILED );

		double nextTime = currentTime;
		if ( controlLaw != 0 )
			nextTime += controlLaw->getSamplingTime( );
		
		if ( preparationStep( nextTime,_yRef ) != SUCCESSFUL_RETURN )
			return ACADOERROR( RET_CONTROLLER_STEP_FAILED );
		
		clock.stop();
		logCollection.setLast( LOG_TIME_CONTROL_LAW,clock.getTime() );
	}
	else
	{
		/* 1) Call Estimator */
		clock.reset();
		clock.start();

		Vector xEst, pEst;

		if ( obtainEstimates( currentTime,_y,xEst,pEst ) != SUCCESSFUL_RETURN )
			return ACADOERROR( RET_CONTROLLER_STEP_FAILED );

		clock.stop();
		logCollection.setLast( LOG_TIME_ESTIMATOR,clock.getTime() );

		/* 2) Evaluate reference trajectory */
		clock.reset();
		clock.start();

		VariablesGrid yRef( _yRef );
		getCurrentReference( currentTime,yRef );

		if ( controlLaw->step( currentTime,xEst,pEst,yRef ) != SUCCESSFUL_RETURN )
			return ACADOERROR( RET_CONTROLLER_STEP_FAILED );
		
		clock.stop();
		logCollection.setLast( LOG_TIME_CONTROL_LAW,clock.getTime() );
	}

	// stop real runtime measurement
	realClock.stop( );
	logCollection.setLast( LOG_TIME_CONTROLLER,realClock.getTime() );

	return SUCCESSFUL_RETURN;
}