File content as of revision 3:c169d08a9d0b:

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

PidController::PidController(){
    elapsedTime =0;
    mode = MANUAL;
    }
    
int PidController::Calculate(float SP, float PV) 
    {
        float CV;                   //(mm/s) Control Variable
        float IntegralAction;       // Integral Contribution to Output
        float DerivativeAction;     // Derivative Contribution to Output
    
        if (mode == MANUAL)
        {   
            CV = SP;
            accumError = 0;
            lastError = 0;
        }
        else 
        {   
            //Calc error
            error = SP -PV;  
            IntegralAction = K_i*(accumError + error);
            DerivativeAction = K_d*(error - lastError);
            
            //-- PID Calculation
            CV = bias + K_p*error, + IntegralAction - DerivativeAction;
            
            //-- Clamp Integral if Output is Saturated
            if ((CV > maxLimit) || (CV < minLimit))
            {
                accumError = accumError;
                }
            else
            {
                accumError += error;
                }
            
            //-- Set Error for Next Scan
            lastError = error;  
        }
           
        
        //Check to See Output is Within Limits
        if (CV > maxLimit){CV= maxLimit;}
        if (CV < minLimit){CV= minLimit;}
        
        
        if (collectDiagnostics){BuildDiagMessage(SP,PV, CV, K_p*error, IntegralAction, DerivativeAction);}
        
        //Convert from mm/s to 0-100%
        return (int)(CV*scalar);
    }

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::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");}
    }