File content as of revision 23:b79faf58d4a1:

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


void initCaptures(void);

//led pins
DigitalOut led1(P2_6);
DigitalOut led2(P2_7);
DigitalOut led3(P2_8);
AnalogIn hallSensor(P0_26);
float P=0,I=0,D=0;
int pulsesMotor2=0,pulsesMotor1=0;
int int_time=0,bint_time=0;
//InterruptIn hall1_(P0_5), hall2_(P0_4);


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

Ticker infoTicker,commandTicker,swaveTicker;

Timer timer;
Timer timer2;


MPU9150 imu(P0_28, P0_27, P2_13);

//Hallsensor hall2(P0_4, P0_5);
Motor motor2(P2_2, P2_3, pulsesMotor2);

//Hallsensor hall1(P1_26, P1_27);

Motor motor1(P2_0, P2_1, pulsesMotor1);
char buffer[200];
void infoTask(void)
{
    led1=!led1;
}
int str2int(char* str)
{
    int num = 0, i = 0;
    while(str[i] != '\n') {
        num = num * 10 + str[i] - '0';
        i++;
    }
    return num;
}

bool squarewave=0;
void swaveTask(void)
{
    if(squarewave) {
        motor2.setAngle(0);
    } else {
        motor2.setAngle(60);
    }
    squarewave=!squarewave;
}
float pidStep=1000.0f;
void getCommand(void)
{
    if(debug.readable()) {
        char buffer[128];
        debug.gets(buffer, 20);
        if(buffer[0]=='m') {
            float speed = (float)(str2int(buffer+3));
            debug.printf("FLOAT %f FLOAT ",speed);
            if(buffer[1]=='1') {
                motor1.setAngle(speed);
            }

            if(buffer[1]=='2') {

                motor2.setAngle(speed);
            }
        } else if(buffer[0]=='p') {
            if(buffer[1]=='+') {
                P+=1/pidStep;
            }

            if(buffer[1]=='-') {
                P-=1/pidStep;
            }
            motor2.setPID(P,I,D);
        } else if(buffer[0]=='i') {
            if(buffer[1]=='+') {
                I+=1/pidStep;
            }
            if(buffer[1]=='-') {
                I-=1/pidStep;
            }
            motor2.setPID(P,I,D);
        } else if(buffer[0]=='d') {
            if(buffer[1]=='+') {
                D+=1/pidStep;
            }

            if(buffer[1]=='-') {
                D-=1/pidStep;
            }
            motor2.setPID(P,I,D);
        } else if(buffer[0]=='s') {
            pidStep = (float)(str2int(buffer+2));
        }
    }
}
int main()
{

    RN42.baud(9600);
    debug.baud(115200);
    //InitTimer0();
    //initialize_connection();
    infoTicker.attach(infoTask,0.2f);
   // commandTicker.attach(getCommand,0.2f);
    swaveTicker.attach(swaveTask,1.0f);
    if(imu.isReady()) {
        debug.printf("MPU9150 is ready\r\n");
    } else {
        debug.printf("MPU9150 initialisation failure\r\n");
    }

    imu.initialiseDMP();

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

    //motor2.setVelocity(0.7);
    imu.setFifoReset(true);
    imu.setDMPEnabled(true);

    Quaternion q1;
    wait_ms(500);
    motor1.setAngle(0);

    motor2.setAngle(0);
    initCaptures();


    while(1) {
        if(imu.getFifoCount() >= 48) {
            imu.getFifoBuffer(buffer,  48);
            led2 = !led2;
        }

        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: %.2f m2: %.2f ",motor1.getAngle(),motor2.getAngle());
            debug.printf("dt1: %f dt2: %f ",motor1.get_dt(),motor2.get_dt());
            debug.printf("P: %f I: %f I: %f  \r\n ",P,I,D);
            getCommand();
            /*  if(!motor1.isRunning()) {
                  if(motor1.getAngle()>30) {
                      motor1.setAngle(0);
                  }
                  if(motor1.getAngle()<30) {
                      motor1.setAngle(60);
                  }
              }
              if(!motor2.isRunning()) {
                  if(motor2.getAngle()>30) {
                      motor2.setAngle(0);
                  }
                  if(motor2.getAngle()<30) {
                      motor2.setAngle(60);
                  }
              }*/
        }
    }

}




void TIMER2_IRQHandler(void)
{
    //   timer2.reset();
    int bitB = (LPC_GPIO0->FIOPIN>>5) & 0x1;
    if(bitB) {
        pulsesMotor2--;
        int_time=0;
    } else {
        pulsesMotor2++;
        int_time=1;
    }

    LPC_TIM2->IR |= 0x10;
    //bint_time=timer2.read_us();
}

void TIMER0_IRQHandler(void)
{
    int bitB = (LPC_GPIO1->FIOPIN>>27) & 0x1;
    if(bitB) {
        pulsesMotor1--;
    } else {
        pulsesMotor1++;
    }

    LPC_TIM0->IR |= 0x10;
}

void initCaptures(void)
{
    NVIC_SetPriority (TIMER3_IRQn, 6);
    LPC_PINCON->PINSEL0  |= 0x00000300; // set P0.4 to CAP2.0
    LPC_SC->PCONP |= (1 << 22);         // Timer2 power on
    // init Timer 2 (cap2.0)
    LPC_TIM2->IR = 0x10;          // clear interrupt register
    NVIC_SetVector(TIMER2_IRQn, uint32_t(TIMER2_IRQHandler));
    LPC_TIM2->TC = 0;                   // clear timer counter
    LPC_TIM2->PC = 0;                   // clear prescale counter
    LPC_TIM2->PR = 0;                   // clear prescale register
    LPC_TIM2->CCR |= 0x5;       // enable cap2.0 rising capture; interrupt on cap2.0
    NVIC_SetPriority (TIMER2_IRQn, 5);
    LPC_TIM2->TCR = (1 << 0);          // start Timer2
    NVIC_EnableIRQ(TIMER2_IRQn);

    LPC_PINCON->PINSEL3  |= 0x00300000; // set P1.26 to CAP0.0
    LPC_SC->PCONP |= (1 << 1);         // Timer0 power on
    // init Timer 0 (cap0.0)
    LPC_TIM0->IR = 0x10;          // clear interrupt register
    NVIC_SetVector(TIMER0_IRQn, uint32_t(TIMER0_IRQHandler));
    LPC_TIM0->TC = 0;                   // clear timer counter
    LPC_TIM0->PC = 0;                   // clear prescale counter
    LPC_TIM0->PR = 0;                   // clear prescale register
    LPC_TIM0->CCR |= 0x5;       // enable cap0.0 rising capture; interrupt on cap0.0
    NVIC_SetPriority (TIMER0_IRQn, 5);
    LPC_TIM0->TCR = (1 << 0);          // start Timer0
    NVIC_EnableIRQ(TIMER0_IRQn);

}