File content as of revision 3:dfb6733ae397:

#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 
SPI deSpi(p5, p6, p7);
Pixy pixy(Pixy::SPI, p11, p12, p13);
Serial pc(USBTX, USBRX); // PC serial channel
Serial bt(p9, p10); // Bluetooth serial channel

// Other
PwmOut leftPwm(p21); 
PwmOut rightPwm(p22); 
InterruptIn bumper(p8);

// Generic PI controller function used by the sensor control thread
void PI(float error, float *output, float *integral, float kP, float kI, float bound) {
    
    // 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;   
    } 
}

// Converts measurements from the QE2 to rads/sec
float QE2RadsPerSec(short counts, short time) {  
    int cnt32;   
    if (counts & 0x00008000) {
        cnt32 = counts | 0xFFFF0000;
    } else {
        cnt32 = counts;
    }   
    return (cnt32*122.62)/time;
}