James MacLean / Robot_Control

The subsystem design/basis for the final project

Dependencies:   mbed-rtos mbed-src pixylib

PeriodicPI.cpp

Committer:
balsamfir
Date:
2017-04-24
Revision:
19:05b8123905fb
Parent:
17:47e107f9587b

File content as of revision 19:05b8123905fb:

#include "PeriodicPI.h"

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

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

    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;
}

void PeriodicPI::Reset(void)
{
    integral = 0;    
}