File content as of revision 10:09dbd00164b9:

#include "mbed.h"
#include "MPU9150.h"
#include "Quaternion.h"
#include "Motor.h"


void InitTimer0(void);

//led pins
DigitalOut led1(P2_6);
DigitalOut led2(P2_7);
DigitalOut led3(P2_8);
AnalogIn hallSensor(P0_26);

volatile int pulses_ = 0;
InterruptIn hall1_(P0_5), hall2_(P0_4);
//Hallsensor hall1(P1_26, P1_27);
//Hallsensor hall2(P0_4, P0_5);

Motor motor1(P2_0, P2_1);
//Motor motor2(P2_2, P2_3, &pulses_);


MPU9150 imu(P0_28, P0_27, P2_13);


Serial RN42(P0_10, P0_11);
Serial debug(P0_2, P0_3);

Ticker infoTicker;

Timer timer;
Timer timer2;



char buffer[200];
void infoTask(void)
{
    led1=!led1;
}
void deneme(void)
{
    led3=!led3;
}
int main()
{
    //Workout what the current state is.
    int h1 = hall1_.read();
    int h2 = hall2_.read();
   // int h3 = hall3_.read();

    // Set the PullUp Resistor
    hall1_.mode(PullUp);
    hall2_.mode(PullUp);
    //hall3_.mode(PullUp);

    //set interrupts on only hall 1-3 rise and fall.
    hall1_.rise(&deneme);
    hall1_.fall(&deneme);
    hall2_.rise(&deneme);
    hall2_.fall(&deneme);
    //hall3_.rise(this, &deneme);
   // hall3_.fall(this, &deneme);
    RN42.baud(9600);
    debug.baud(115200);
   // InitTimer0();
    // initialize_connection();

    infoTicker.attach(infoTask,0.2f);

    if(imu.isReady()) {
        debug.printf("MPU9150 is ready\r\n");
    } else {
        debug.printf("MPU9150 initialisation failure\r\n");
    }

    imu.initialiseDMP();

    timer.start();
    timer2.start();

    imu.setFifoReset(true);
    imu.setDMPEnabled(true);

    Quaternion q1;
    while(1) {

        if(imu.getFifoCount() >= 48) {
            imu.getFifoBuffer(buffer,  48);
            led2 = !led2;
            //debug.printf("%d\r\n",timer2.read_ms());
            timer2.reset();
        }

        if(timer.read_ms() > 50) {
            timer.reset();

            //This is the format of the data in the fifo,
            /* ================================================================================================ *
              | Default MotionApps v4.1 48-byte FIFO packet structure:                                           |
              |                                                                                                  |
              | [QUAT W][      ][QUAT X][      ][QUAT Y][      ][QUAT Z][      ][GYRO X][      ][GYRO Y][      ] |
              |   0   1   2   3   4   5   6   7   8   9  10  11  12  13  14  15  16  17  18  19  20  21  22  23  |
              |                                                                                                  |
              | [GYRO Z][      ][MAG X ][MAG Y ][MAG Z ][ACC X ][      ][ACC Y ][      ][ACC Z ][      ][      ] |
              |  24  25  26  27  28  29  30  31  32  33  34  35  36  37  38  39  40  41  42  43  44  45  46  47  |
              * ================================================================================================ */

            /*
            debug.printf("%d, %d, %d\r\n",  (int32_t)(((int32_t)buffer[34] << 24) + ((int32_t)buffer[35] << 16) + ((int32_t)buffer[36] << 8) + (int32_t)buffer[37]),
                                            (int32_t)(((int32_t)buffer[38] << 24) + ((int32_t)buffer[39] << 16) + ((int32_t)buffer[40] << 8) + (int32_t)buffer[41]),
                                            (int32_t)(((int32_t)buffer[42] << 24) + ((int32_t)buffer[43] << 16) + ((int32_t)buffer[44] << 8) + (int32_t)buffer[45]));

            debug.printf("%d, %d, %d\r\n",  (int32_t)(((int32_t)buffer[16] << 24) + ((int32_t)buffer[17] << 16) + ((int32_t)buffer[18] << 8) + (int32_t)buffer[19]),
                                            (int32_t)(((int32_t)buffer[20] << 24) + ((int32_t)buffer[21] << 16) + ((int32_t)buffer[22] << 8) + (int32_t)buffer[23]),
                                            (int32_t)(((int32_t)buffer[24] << 24) + ((int32_t)buffer[25] << 16) + ((int32_t)buffer[26] << 8) + (int32_t)buffer[27]));

            debug.printf("%d, %d, %d\r\n",  (int16_t)(buffer[29] << 8) + buffer[28],
                                            (int16_t)(buffer[31] << 8) + buffer[30],
                                            (int16_t)(buffer[33] << 8) + buffer[32]);

            debug.printf("%f, %f, %f, %f\r\n",
                (float)((((int32_t)buffer[0] << 24) + ((int32_t)buffer[1] << 16) + ((int32_t)buffer[2] << 8) + buffer[3]))* (1.0 / (1<<30)),
                (float)((((int32_t)buffer[4] << 24) + ((int32_t)buffer[5] << 16) + ((int32_t)buffer[6] << 8) + buffer[7]))* (1.0 / (1<<30)),
                (float)((((int32_t)buffer[8] << 24) + ((int32_t)buffer[9] << 16) + ((int32_t)buffer[10] << 8) + buffer[11]))* (1.0 / (1<<30)),
                (float)((((int32_t)buffer[12] << 24) + ((int32_t)buffer[13] << 16) + ((int32_t)buffer[14] << 8) + buffer[15]))* (1.0 / (1<<30)));
            */

            q1.decode(buffer);

            debug.printf("%f, ",hallSensor.read());
            debug.printf("%f, %f, %f, %f ", q1.w, q1.v.x, q1.v.y, q1.v.z);

            Vector3 vector_x(1,0,0),vector_y(0,1,0),vector_z(0,0,1);
            Vector3 pry=q1.getEulerAngles();
            debug.printf("p: %f r: %f y: %f ",pry.x/PI*180,pry.y/PI*180,pry.z/PI*180);
           /* debug.printf("m1: %d m2: %d \r\n ",motor1.getPulses(),motor2.getPulses());
            motor1.setVelocity(pry.x/90);
            motor2.setVelocity(pry.x/90);*/

        }
    }
}

/*/////////////////////////////////////////////////////////////////////////////////////////////////////////
//  TIMER0 / CAPTURE0[0]
/////////////////////////////////////////////////////////////////////////////////////////////////////////

    int          prevState_=0;
    int          currState_=0;
void TIMER0_IRQHandler __irq (void){
    int h1   = (LPC_GPIO1->FIOPIN>>26) & 0x1;
    int h2   = (LPC_GPIO1->FIOPIN>>27) & 0x1;
    //2-bit state.
    currState_ = (h1 << 2) | (h2 << 1);

    if ((prevState_ == 0x0) && (currState_ == 0x2)) {
        pulses_++;
        prevState_ = currState_;
        return;
    } else if (prevState_ == 0x0 && currState_ == 0x1) {
        pulses_--;
        prevState_ = currState_;
        return;
    }

    if ((prevState_ == 0x2) && (currState_ == 0x3)) {
        pulses_++;
        prevState_ = currState_;
        return;
    } else if (prevState_ == 0x2 && currState_ == 0x0) {
        pulses_--;
        prevState_ = currState_;
        return;
    }
    if ((prevState_ == 0x1) && (currState_ == 0x0)) {
        pulses_++;
        prevState_ = currState_;
        return;
    } else if (prevState_ == 0x1 && currState_ == 0x3) {
        pulses_--;
        prevState_ = currState_;
        return;
    }
    
    if ((prevState_ == 0x3) && (currState_ == 0x1)) {
        pulses_++;
        prevState_ = currState_;
        return;
    } else if (prevState_ == 0x3 && currState_ == 0x2) {
        pulses_--;
        prevState_ = currState_;
        return;
    }

    prevState_ = currState_;
    if( LPC_TIM0->IR & 0x10 ) {
        LPC_TIM0->IR = 0x10;          // Clear MATx interrupt include DMA request
    }
    if( LPC_TIM0->IR & 0x20 ) {
        LPC_TIM0->IR = 0x20;          // Clear MATx interrupt include DMA request
    }

}

void InitTimer0(void)
{
    //hold timer0 in reset
    LPC_TIM0->TCR=2;

    //timer0 power on
    LPC_SC->PCONP |= (0x01<<1);

    //cap0.0 cap0.1 pin input
    LPC_PINCON->PINSEL3 &= ~((0x3<<20)|(0x3<<22));
    LPC_PINCON->PINSEL3 |= (0x3<<20)|(0x3<<22);

    //capture interrupts clear
    LPC_TIM0->IR = 0x30;          // Clear MATx interrupt include DMA request


    //timer0 pid de kullanildigindan prescaler iptal. counter frekansi 18m olmali.
    //LPC_TIM0->PR  = pclk/1000000; // set prescaler to get 1 M counts/sec

    //Capture 0 and 1 on rising edge, interrupt enable.
    LPC_TIM0->CCR = 0x3f;//(0x1<<0) |  (0x1<<1) |  (0x1<<2) | (0x1<<3) |  (0x1<<4) |( 0x1<<5);

    NVIC_EnableIRQ(TIMER0_IRQn);

    //start timer0
    LPC_TIM0->TCR=1;
}*/