File content as of revision 5:1206105e20bd:

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

PidController::PidController(char c){
    elapsedTime =0;
    mode = MANUAL;
    diagChar = c;
    }
    
float 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;
            prevError = 0;
        }
        else 
        {   
            //Calc error
            error = SP - PV;  
            IntegralAction = K_i*(accumError + error);
            //DerivativeAction = K_d*(PV - lastInput);
            DerivativeAction = K_d*(error - prevError);
            
            //-- PID Calculation
            if (SP)
            { 
                CV =  bias + K_p*error + IntegralAction + DerivativeAction;
                if (CV>0) {CV = sqrt(CV);}
            }
            else 
            {
                CV= 0;
                accumError = 0;
            }
            
            //-- Only allow the Controller to integrate if the output isnt saturated
            if ((CV < maxLimit) || (CV > minLimit))
            {
                accumError += error;
                }

            //-- Save Current Input for Next Loop
            //lastInput = PV;  
            prevError = error;
            
            //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 CV;
    }

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

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


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

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

void PidController::BuildDiagMessage(float SetPoint, float ProcessVar, float PWM, float PropAction, float IntAction, float DifAction){
    //sprintf(diagMsg, "%c %d %0.4f %0.4f %0.1f %0.4f %0.4f %0.4f\n", diagChar, elapsedTime, SetPoint, ProcessVar, PWM, PropAction, IntAction, DifAction);
    sprintf(diagMsg, "%c %d %0.4f %0.4f %0.1f 0.0 0.0 0.0\n", diagChar, elapsedTime, SetPoint, ProcessVar, PWM); 
    elapsedTime += RATE;
    return;
    }