File content as of revision 4:b590bd8fec6f:

#include "mbed.h"
#include "PidController.h"

PidController::PidController(bool sqrt){
    elapsedTime =0;
    mode = MANUAL;
    squareRootOuptut = sqrt;
    }
    
int PidController::Calculate(float SP, float PV, float ManualMV) 
    {
        float CV;                   //(mm/s) Control Variable
        float IntegralAction;       // Integral Contribution to Output
        float DerivativeAction;     // Derivative Contribution to Output
    
        if (mode == MANUAL)
        {   
            CV = ManualMV;      //Write Manual Manipulated Variable
            accumError = 0;
        }
        else 
        {   
            //Calc error
            error = SP - PV;  
            IntegralAction = K_i*(accumError + error);
            DerivativeAction = K_d*(PV - lastInput);
            
            //-- PID Calculation
            if (SP) { 
                CV =  K_p*error + IntegralAction - DerivativeAction;
                if ((CV > 0) &&  squareRootOuptut)CV = sqrt(CV);
                }
            else 
            
            CV= 0;
            
                
            //-- Only allow the Controller to integrate if the output isnt saturated
            if ((CV < maxLimit) || (CV > minLimit))
            {
                accumError += error;
                }
            
            //-- Told to stop!
            if (!SP) accumError = 0;

            //-- Save Current Input for Next Loop
            lastInput = PV;  
            
            //Check to See Output is Within Limits
            if (CV > maxLimit){CV= maxLimit;}
            if (CV < minLimit){CV= minLimit;}
        }
           
        
        //-- Make message to send to GUI
        if (collectDiagnostics){BuildDiagMessage(SP,PV, CV, K_p*error, IntegralAction, DerivativeAction);}
        
        return (int)(CV);
    }

void PidController::UpdateSettings(float Bias, float PropGain, float IntGain, float DiffGain, float OutputMin, float OutputMax, float OutputScale){
    bias = Bias;
    K_p = PropGain;
    K_i = IntGain;
    K_d = DiffGain;
    minLimit = OutputMin;
    maxLimit = OutputMax;
    scalar = OutputScale;
    }

void PidController::UpdateSettings(float OutputMin, float OutputMax){
    minLimit = OutputMin;
    maxLimit = OutputMax;
    }


void PidController::StartDiag(void){
    elapsedTime =0;
    collectDiagnostics = true;
    }

void PidController::EndDiag(void){
    collectDiagnostics = false;

    }

void PidController::BuildDiagMessage(float SetPoint, float ProcessVar, float PWM, float PropAction, float IntAction, float DifAction){
    sprintf(diagMsg, "P %d %0.4f %0.4f %0.1f %0.4f %0.4f %0.4f\n", elapsedTime, SetPoint, ProcessVar, PWM, PropAction, IntAction, DifAction);
    elapsedTime += RATE;
    if (elapsedTime > 32000){sprintf(diagMsg, "R");}
    }