Rauno U / Mbed 2 deprecated Miisu

Six crescent shaped legs

Dependencies:   mbed

PIDController.cpp

Committer:
sim642
Date:
2016-06-21
Revision:
47:4f418a4b0051
Parent:
13:dc5f5f19e4e8

File content as of revision 47:4f418a4b0051:

#include "PIDController.hpp"

PIDController::PIDController(PIDData nData) :
    data(nData)
{
    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 = data.Kp * error + data.Ki * integral + data.Kd * derivative;

    prev_error = error;

    return out;
}

void PIDController::set(PIDData nData)
{
    data = nData;
}