James MacLean / Robot_Control

The subsystem design/basis for the final project

Dependencies:   mbed-rtos mbed-src pixylib

PeriodicPI.cpp@5:f655435d0782, 2016-03-25 (annotated)

Committer:
balsamfir
Date:
Fri Mar 25 13:36:14 2016 +0000
Revision:
5:f655435d0782
Child:
6:52686c25e4af
Backup before revert

Who changed what in which revision?

UserRevisionLine numberNew contents of line
balsamfir 5:f655435d0782 1 #include "PeriodicPI.h"
balsamfir 5:f655435d0782 2
balsamfir 5:f655435d0782 3 PeriodicPI::PeriodicPI(float periodSec, float kP=0, float kI=0)
balsamfir 5:f655435d0782 4 {
balsamfir 5:f655435d0782 5 this->kP = kP;
balsamfir 5:f655435d0782 6 this->kI = kI;
balsamfir 5:f655435d0782 7 this->timescale = timescale;
balsamfir 5:f655435d0782 8 }
balsamfir 5:f655435d0782 9
balsamfir 5:f655435d0782 10 float PeriodicPI::Run(float error, float bound)
balsamfir 5:f655435d0782 11 {
balsamfir 5:f655435d0782 12 // Scale error by the time step
balsamfir 5:f655435d0782 13 error = error * periodSec;
balsamfir 5:f655435d0782 14
balsamfir 5:f655435d0782 15 // Avoid integrator wind up
balsamfir 5:f655435d0782 16 if((output >= bound)||(output <= -bound));
balsamfir 5:f655435d0782 17 else {
balsamfir 5:f655435d0782 18 integral = integral + error;
balsamfir 5:f655435d0782 19 }
balsamfir 5:f655435d0782 20
balsamfir 5:f655435d0782 21 output = kI * (integral) + kP * error;
balsamfir 5:f655435d0782 22
balsamfir 5:f655435d0782 23 // Limit output to bounds
balsamfir 5:f655435d0782 24 if (output > bound) {
balsamfir 5:f655435d0782 25 output = bound;
balsamfir 5:f655435d0782 26 } else if (output < -bound) {
balsamfir 5:f655435d0782 27 output = -bound;
balsamfir 5:f655435d0782 28 }
balsamfir 5:f655435d0782 29
balsamfir 5:f655435d0782 30 return output;
balsamfir 5:f655435d0782 31 }
balsamfir 5:f655435d0782 32
balsamfir 5:f655435d0782 33 float PeriodicPI::GetIntegral()
balsamfir 5:f655435d0782 34 {
balsamfir 5:f655435d0782 35 return integral;
balsamfir 5:f655435d0782 36 }
balsamfir 5:f655435d0782 37
balsamfir 5:f655435d0782 38 float PeriodicPI::GetOutput()
balsamfir 5:f655435d0782 39 {
balsamfir 5:f655435d0782 40 return output;
balsamfir 5:f655435d0782 41 }