N K / Mbed 2 deprecated GaNtroller

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

Fork of priustroller_current by N K

meta/pidcontroller.cpp

Committer:
nki
Date:
2015-03-10
Revision:
25:0003b824dd7d
Parent:
24:f1ff9c7256b5
Child:
50:16b43e8fe04f

File content as of revision 25:0003b824dd7d:

#include "includes.h"
#include "filters.h"

PidController::PidController(float ki, float kp, float kd, float out_max, float out_min) {
    _ki = ki;
    _kp = kp;
    _kd = kd;
    _last_in = 0.0f;
    _integral = 0.0f;
    _out_max = out_max;
    _out_min = out_min;
}

float PidController::Update(float ref, float in) {
    float error = ref - in;

    _integral += error;
    
    if (_integral * _ki > _out_max) _integral = _out_max / _ki;
    if (_integral * _ki < _out_min) _integral = _out_min / _ki;
    
    float deriv = _last_in - in;
    _last_in = in;
    float tmp = _ki * _integral + _kp * error + _kd * deriv;
    
    if (tmp > _out_max) tmp = _out_max;
    if (tmp < _out_min) tmp = _out_min;
    
    return tmp;
}