Matthias Grob / Mbed 2 deprecated FlyBed1

NOT FINISHED YET!!! My first try to get a self built fully working Quadrocopter based on an mbed, a self built frame and some other more or less cheap parts.

Dependencies:   mbed MODI2C

PID/PID.cpp

Committer:
maetugr
Date:
2014-02-14
Revision:
40:2ca410923691
Parent:
33:fd98776b6cc7

File content as of revision 40:2ca410923691:

#include "PID.h"

PID::PID(float P, float I, float D, float Integral_Max)
{
    Integral = 0;
    LastTime = 0;
    SetPoint = 0;
    Integrate = true;
    PID::P = P;
    PID::I = I;
    PID::D = D;
    PID::Integral_Max = Integral_Max;
    dtTimer.start();
}

float PID::compute(float SetPoint, float ProcessValue)
{
    // meassure dt
    float dt = dtTimer.read() - LastTime;    // time in us since last loop
    LastTime = dtTimer.read();                   // set new time for next measurement
    
    // Proportional
    float Error =  ProcessValue - SetPoint;
    
    // Integral
    if (dt > 2 || !Integrate) // Todo: 2 secs is the maximal time between two computations
        Integral = 0;
    else if (abs(Integral + Error) <= Integral_Max)
        Integral += Error * dt;
        
    // Derivative
    float Derivative = (Error - PreviousError) / dt;
    
    // Final Formula
    float Result = P * Error + I * Integral + D * Derivative;
    
    PreviousError = Error;
    
    return Result;
}

void PID::setPID(float P, float I, float D)
{
    PID::P = P;
    PID::I = I;
    PID::D = D;
}

void PID::setIntegrate(bool Integrate)
{
    PID::Integrate = Integrate;
}