File content as of revision 2:2bc519e14bae:

#include "global.h"

// IO Port
DigitalOut led1(LED1);
DigitalOut led2(LED2);
DigitalOut led3(LED3);
DigitalOut led4(LED4);
DigitalOut leftDir(p29);
DigitalOut rightDir(p30);
DigitalOut spiReset(p11);
DigitalOut ioReset(p12);

// Comunication 
PwmOut leftPwm(p21); 
PwmOut rightPwm(p22); 
SPI deSpi(p5, p6, p7);
Serial pc(USBTX, USBRX); // PC serial channel
Serial bt(p9, p10); // Bluetooth serial channel

// Generic PI controller function used by the sensor control thread
void PI(float *xState, float *u, float setPoint, float kP, float kI, float feedback, float bound) {
    float e;
    
    e = setPoint - feedback; 

    // Avoid integrator wind up
    if((*u>=bound)||(*u<=-bound)) ; // turns off integrator when u maxed
    else {
        *xState = *xState + e;
    }

    *u = kI*(*xState) + kP*e;
    
    if (fabs(*u) > bound) {
        *u = bound;
    }
}

// Converts measurements from the QE2 to rads/sec
float QE2RadsPerSec(int counts, int time) {
    return (counts*122.62)/time;
}