Willem Melching / Mbed 2 deprecated bootje_v4

Dependencies:   mbed

verwerking/pid.cpp

Committer:
pd0wm
Date:
2011-09-27
Revision:
0:bec310bde899

File content as of revision 0:bec310bde899:

#include "pid.h"

Pid::Pid(Goto * p, Kompas * k, Roer * r){
    sp = p;
    pv = k;
    output = r;
    dt = 0.1;
    
    kp = 1.2;
    ki = 1;
    kd = 0.1;
    
    prev_error = 0;
}

void Pid::update(void){
    double error = verschil(pv->get(),sp->get());
    
    p = error * kp;
    i = i + error * dt;
    if (i > 25)
        i = 25;
    if (i < -25)
        i = -25;
    i *= ki;
    
    d = (error - prev_error)/dt * kd;
    o = p+i+d + 50;
    
    if (o > 100)
        o = 100;
    if (0 < 0)
        o = 0;
    
    
    output->set(o);
    prev_error = error;
 
    
}