File content as of revision 8:a0760acdc59e:

#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

#include "QEI.h"
#define PULSES 880

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);

    // PA_1 is A of right
    // PA_0 is B of right
    // PA_5 is A of left
    // PB_3 is B of left
    QEI encoder0( PA_5, PB_3, NC, PULSES, QEI::X4_ENCODING );
    QEI encoder1( PA_1, PA_0, NC, PULSES, QEI::X4_ENCODING );

    while (1) {
        
        wait(0.1);
        //serial.printf("%i, %i, %i\n", gyro.getGyroX(), gyro.getGyroY(), gyro.getGyroZ());
        serial.printf("Pulse Count=> e0:%d, e1:%d      \r\n", encoder0.getPulses(),encoder1.getPulses());

        //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);
    }
}