File content as of revision 5:9332620c08c6:

#include "mbed.h"
#include "measuring_wheel.h"

Serial pc(USBTX, USBRX, 115200);
MeasuringWheel measuring(PA_9, PA_8, PA_0, PA_1, PB_7, PB_6);
// MeasuringWheel measuring(PB_6,PB_7,PA_8,PA_9,PA_6,PA_7);

int main() {
  measuring.wheelDiameter(48, 48, 48);
  while (true) {
    // pc.printf("%d,%d,\n",(int)measuring.getOutX(),(int)measuring.getOutY());
    //        pc.printf("w1 = %f, w2 = %f, w3 =
    //        %f\r\n",measuring.getWheel1(),measuring.getWheel2(),measuring.getWheel3());
    // pc.printf("yaw = %f\r\n",measuring.getjyroAngle());
    /// Thread::wait(100);
    // measuring.debugLed[1]->write(!measuring.debugLed[1]->read());
    if (abs((int)measuring.getjyroAngle()) % 6 > 3) {
      measuring.debugLed[1]->write(true);
    } else {
      measuring.debugLed[1]->write(false);
    }
    if (abs((int)measuring.getOutX()) % 6 > 3) {
      measuring.debugLed[2]->write(true);
    } else {
      measuring.debugLed[2]->write(false);
    }
    // kwait(fabs(measuring.getjyroAngle()) / 1.0);
    wait(0.1);
  }
}