/*=====================================================================================
 File name:        RMP3CNTL.C  (IQ version)                  
                    
 Originator:	Digital Control Systems Group
			Texas Instruments

 Description:  The ramp3 down control                   

=====================================================================================
 History:
-------------------------------------------------------------------------------------
 04-15-2005	Version 3.20
-------------------------------------------------------------------------------------*/
#include "dmctype.h"
#include "IQmathLib.h"         // Include header for IQmath library

#include "rmp_cntl_v2.h"

/*
 * PosRampPlus
 * PosRampMinus
 * NegRampPlus
 * NegRampMinus
 */

#pragma CODE_SECTION(rmp_cntl_v2_calc,".fast_run");
void rmp_cntl_v2_calc(RMP_V2 *v)
{
	_iq tmp;
	
	tmp = v->DesiredInput - v->Out;

	if (v->Out>=0)
	{
	// для положительной части


	    if (v->Out >= v->RampHighLimit1)
	    {
	        // мы на втором уровне
	        if (tmp > (_iq)v->PosRampPlus2)
	        {
	            v->Out += v->PosRampPlus2;
	            if (v->Out > v->RampHighLimit)
	                v->Out = v->RampHighLimit;
	        }
	        else
	        {
	            if (tmp < (_iq)v->PosRampMinus2)
	            {
	                v->Out += (_iq)v->PosRampMinus2;
	                if (v->Out < v->RampLowLimit)
	                    v->Out = v->RampLowLimit;
	            }
	            else
	            {
	                v->Out  = v->DesiredInput;
	            }
	        }


	    }
	    else
	    {
	        // мы на первом уровне
            if (tmp > (_iq)v->PosRampPlus1)
            {
                v->Out += v->PosRampPlus1;
                if (v->Out > v->RampHighLimit)
                    v->Out = v->RampHighLimit;
            }
            else
            {
                if (tmp < (_iq)v->PosRampMinus1)
                {
                    v->Out += (_iq)v->PosRampMinus1;
                    if (v->Out < v->RampLowLimit)
                        v->Out = v->RampLowLimit;
                }
                else
                {
                    v->Out  = v->DesiredInput;
                }
            }

	    }

//        if (tmp > (_iq)v->PosRampPlus)
//        {
//            //v->RampDoneFlag = 0;
//            v->Out += v->PosRampPlus;
//            if (v->Out > v->RampHighLimit)
//                v->Out = v->RampHighLimit;
//        }
//        else
//        {
//            if (tmp < (_iq)v->PosRampMinus)
//            {
//                //v->RampDoneFlag = 0;
//                v->Out += (_iq)v->PosRampMinus;
//                if (v->Out < v->RampLowLimit)
//                    v->Out = v->RampLowLimit;
//            }
//            else
//            {
//                v->Out  = v->DesiredInput;
//                //v->RampDoneFlag = 0x7FFFFFFF;
//            }
//        }

	}
	else
	{
	    // для отриц. части


	    if (v->Out <= v->RampLowLimit1)
	    {
	        // мы на втором уровне
            if (tmp > (_iq)v->NegRampPlus2)
            {
                v->Out += v->NegRampPlus2;
                if (v->Out > v->RampHighLimit)
                    v->Out = v->RampHighLimit;
            }
            else
            {
                if (tmp < (_iq)v->NegRampMinus2)
                {
                    v->Out += (_iq)v->NegRampMinus2;
                    if (v->Out < v->RampLowLimit)
                        v->Out = v->RampLowLimit;
                }
                else
                {
                    v->Out  = v->DesiredInput;
                }
            }

	    }
	    else
	    {
	        // мы на первом уровне
            if (tmp > (_iq)v->NegRampPlus1)
            {
                v->Out += v->NegRampPlus1;
                if (v->Out > v->RampHighLimit)
                    v->Out = v->RampHighLimit;
            }
            else
            {
                if (tmp < (_iq)v->NegRampMinus1)
                {
                    v->Out += (_iq)v->NegRampMinus1;
                    if (v->Out < v->RampLowLimit)
                        v->Out = v->RampLowLimit;
                }
                else
                {
                    v->Out  = v->DesiredInput;
                }
            }

	    }

//	        if (tmp > (_iq)v->NegRampPlus)
//	        {
//	            //v->RampDoneFlag = 0;
//	            v->Out += v->NegRampPlus;
//	            if (v->Out > v->RampHighLimit)
//	                v->Out = v->RampHighLimit;
//	        }
//	        else
//	        {
//	            if (tmp < (_iq)v->NegRampMinus)
//	            {
//	                //v->RampDoneFlag = 0;
//	                v->Out += (_iq)v->NegRampMinus;
//	                if (v->Out < v->RampLowLimit)
//	                    v->Out = v->RampLowLimit;
//	            }
//	            else
//	            {
//	                v->Out  = v->DesiredInput;
//	                //v->RampDoneFlag = 0x7FFFFFFF;
//	            }
//	        }

	}

}