Rauno U / Mbed 2 deprecated Miisu

Six crescent shaped legs

Dependencies:   mbed

PIDController.cpp

Committer:
sim642
Date:
2016-03-29
Revision:
2:cf0147952fb9
Child:
3:2235787e78c4

File content as of revision 2:cf0147952fb9:

#include "PIDController.hpp"

PIDController::PIDController() : Kp(0), Ki(0), Kd(0)
{
    reset();
}

PIDController::~PIDController()
{

}

void PIDController::reset()
{
    timer.reset();
    timer.start();
    prev_error = 0;
    integral = 0;
}

float PIDController::step(float error)
{
    float dt = timer.read();
    timer.reset(); // timer keeps running

    integral += error * dt;
    float derivative = (error - prev_error) / dt;
    float out = Kp * error + Ki * integral + Kd * derivative;

    prev_error = error;

    return out;
}

void PIDController::set(float nKp, float nKi, float nKd)
{
    Kp = nKp;
    Ki = nKi;
    Kd = nKd;
}