File content as of revision 0:8adde5eaea5a:

#include "mbed.h"
#include "actions.h"
        
int main() {
    pid lman, rman;
    lman.kp = .5;
    lman.ki = .1;
    lman.kd = .4;
    rman.kp = .5;
    rman.ki = .1;
    rman.kd = .4;
    //INITIALIZATION
    while(1) {
       float left_encoder = lfront.read();
       float right_encoder = rfront.read();
       float bl = lback.read();
       float br = rback.read();
        printf("Left Encoder: %.2f %%\n", lfront.read());
        printf("Right Encoder: %.2f %%\n", rfront.read());
        printf("BL Encoder: %.2f %%\n", lback.read());
        printf("BR Encoder: %.2f %%\n", rback.read());
        
        float error = left_encoder - right_encoder; //Can be pos or neg
        if(error == 0){
            resetpid(lman);
            resetpid(rman);
        }
        float adjust_r = getFix(rman, error); //Only right wheel adjusted for this example, we can change this later / make it more complicated
    }
}