File content as of revision 7:6f5cb6377bd4:

#include "mbed.h"

#include "irpair.h"
#include "main.h"
#include "motor.h"

#include <stdlib.h>
#include "ITG3200.h"
#include "QEI.h"

// PID
#define P_CONSTANT 0.0025
#define I_CONSTANT 0.0000025
#define D_CONSTANT 0.25


int main()
{
    enableMotors();
    //Set highest bandwidth.
    gyro.setLpBandwidth(LPFBW_42HZ);
    serial.baud(9600);
    serial.printf("The gyro's address is %s", gyro.getWhoAmI());

    wait (0.1);

//    IR_1.write(1);
//    IR_2.write(1);
//    IR_3.write(1);
//    IR_4.write(1);

    redLed.write(1);
    greenLed.write(0);
    blueLed.write(1);
    
    rightWheelForward(0.1);
    leftWheelForward(0.1);

    while (1) {

    wait(0.1);
        serial.printf("%i, %i, %i\n", gyro.getGyroX(), gyro.getGyroY(), gyro.getGyroZ());

        //reading = Rec_4.read();
//        serial.printf("reading: %f\n", reading);
//        redLed.write(0);
//        wait_ms(1000);
//        redLed.write(1);
//        greenLed.write(0);
//        wait_ms(1000);
//        greenLed.write(1);
//        blueLed.write(0);
//        wait_ms(1000);
//        blueLed.write(1);
    }
}