File content as of revision 0:ce3ac53af6e4:

#include "IMU.h"
#include "PID.h"
//#include "MS5837.h"
#include "Motor.h"

//Declare IMU object
MPU6050 mpu1;

double myPitch, sOut, setPoint;
double k_p, k_i, k_d;

Motor mBlack(D3,D2,D9); // pwm, fwd, rev
Motor mWhite(D4,D5,D8);

//Input, Output, SetPoint, kp, ki, kd, Controller Direction
PID pidp(&myPitch, &sOut, &setPoint, 1, 1, 1, DIRECT);

int main()
{
    i2c.frequency(400000);  // use fast (400 kHz) I2C
    wait(.2);
    //Initialize mpu]
    IMUinit(mpu1);
    pidp.SetMode(AUTOMATIC);    
    pidp.SetOutputLimits(0.5, 1);
    pidp.SetTunings(.028, 0.01, 0.025);
    setPoint = 0;
    
    while(1)
    {    
        //other stuff .. ie state machines  
        IMUPrintData(mpu1);
        myPitch = pitch;
        pidp.Compute();
        float s2Out = 1.5 - sOut;
        mWhite.speed(s2Out); 
        mBlack.speed(-sOut);
        
        char text[60];
        sprintf(text, "%f,%f,%f,%f,%f \n", yaw, pitch, roll,sOut, s2Out);
        pc.printf("%s", text);
        
    }
}