James MacLean / Robot_Control

The subsystem design/basis for the final project

Dependencies:   mbed-rtos mbed-src pixylib

PeriodicPI.cpp

Committer:
balsamfir
Date:
2016-03-25
Revision:
5:f655435d0782
Child:
6:52686c25e4af

File content as of revision 5:f655435d0782:

#include "PeriodicPI.h"

PeriodicPI::PeriodicPI(float periodSec, float kP=0, float kI=0)
{
    this->kP = kP;
    this->kI = kI;
    this->timescale = timescale;
}

float PeriodicPI::Run(float error, float bound) 
{
    // Scale error by the time step
    error = error * periodSec;
    
    // Avoid integrator wind up
    if((output >= bound)||(output <= -bound));
    else {
        integral = integral + error;
    }

    output = kI * (integral) + kP * error;
    
    // Limit output to bounds
    if (output > bound) {
        output = bound;
    } else if (output < -bound) {
        output = -bound;   
    } 
    
    return output;
}

float PeriodicPI::GetIntegral() 
{
    return integral;
}

float PeriodicPI::GetOutput()
{
    return output;
}