PID_Values.P_Term = PID_Values.Percnt_Error;
    PID_Values.P_Term = PID_Parameters.KP * PID_Values.P_Term;


    if(PID_Values.P_Term >= PID_Values.PMAX)
    {
        PID_Values.P_Term = PID_Values.PMAX;
    }
    else if(PID_Values.P_Term <= PID_Values.PMIN)
    {
        PID_Values.P_Term = PID_Values.PMIN;
    }

    PID_Values.PID_Term = PID_Values.P_Term;
    PID_Values.PID_Term = PID_Values.PID_Term + PID_Values.I;
    PID_Values.PID_Term = PID_Values.PID_Term - PID_Values.D_Term;




    if(PID_Values.PID_Term >= PID_Values.PIDMAX)
    {
        PID_Values.PID_Term = PID_Values.PIDMAX;
    }
    else if(PID_Values.PID_Term <= PID_Values.PIDMIN)
    {
        PID_Values.PID_Term = PID_Values.PIDMIN;
    }

    if (PID_Values.PID_Term<=0)
    {
        PID_Values.PIDOutput = 0;
    }
    else
    {
        PID_Values.PIDOutput = (PID_Values.PID_Term/PID_Values.PIDMAX)*1000;
    }



    PID_Values.Percnt_Error1    = PID_Values.Percnt_Error;
    PID_Values.ref_freq         = (U16) 16*PID_Values.PIDOutput;

    **Integral/Derivative Calculation**

PID_Values.Error_Accum = PID_Values.Error_Accum + PID_Values.Percnt_Error;  // Error Accumulation over time


            if(PID_Values.Error_Accum >= PID_Values.Error_AccumMAX)
            {
                PID_Values.Error_Accum = PID_Values.Error_AccumMAX;
            }

            if(PID_Values.Error_Accum <= PID_Values.Error_AccumMIN)
            {
                PID_Values.Error_Accum = PID_Values.Error_AccumMIN;
            }

//          NumToString(PID_Values.Error_Accum, (char*)temp1);
//          printLcpLine(0, 33, 127, 10, (const U8*)temp1, CENTER);

            PID_Values.I =  PID_Values.Error_Accum* PID_Values.Integral_Time;//(PID_Parameters.IntergrlTime);

            if(PID_Values.I >= PID_Values.IMAX)
            {
                PID_Values.I = PID_Values.IMAX;
            }
            else if(PID_Values.I <= PID_Values.IMIN)
            {
                PID_Values.I = PID_Values.IMIN;
            }
//          NumToString(PID_Values.I, (char*)temp1);
//          printLcpLine(0, 23, 127, 10, (const U8*)temp1, CENTER);

            // Derivative calculation

            PID_Values.D_Term = PID_Parameters.DiffTime * (PID_Values.Percnt_Error - PID_Values.Old_Error);

            PID_Values.Old_Error = PID_Values.Percnt_Error1;
            if(PID_Values.D_Term >= PID_Values.DMAX)
            {
                PID_Values.D_Term = PID_Values.DMAX;
            }
            else if(PID_Values.D_Term <=PID_Values.DMIN)
            {
                PID_Values.D_Term = PID_Values.DMIN;
            }