first publish
Dependencies: MPU9150_DMP MotorControl QuaternionMath SimpleIOMacros mbed
Fork of cool_step by
Diff: main.cpp
- Revision:
- 18:9f249b8a59ae
- Parent:
- 17:b3acd6416356
- Child:
- 19:e89f44519f6b
--- a/main.cpp Sat May 16 08:50:02 2015 +0000 +++ b/main.cpp Sat May 16 11:12:40 2015 +0000 @@ -4,11 +4,14 @@ #include "Motor.h" +void initCaptures(void); + //led pins DigitalOut led1(P2_6); DigitalOut led2(P2_7); DigitalOut led3(P2_8); AnalogIn hallSensor(P0_26); +int pulsesMotor2=0,pulsesMotor1=0; //InterruptIn hall1_(P0_5), hall2_(P0_4); @@ -21,15 +24,14 @@ Timer timer2; - MPU9150 imu(P0_28, P0_27, P2_13); -Hallsensor hall2(P0_4, P0_5); -Motor motor2(P2_2, P2_3, &hall2); +//Hallsensor hall2(P0_4, P0_5); +Motor motor2(P2_2, P2_3, NULL); //Hallsensor hall1(P1_26, P1_27); -//Motor motor1(P2_0, P2_1,&hall1); +Motor motor1(P2_0, P2_1, NULL); char buffer[200]; void infoTask(void) { @@ -41,20 +43,21 @@ } int main() { - + RN42.baud(9600); debug.baud(115200); //InitTimer0(); - //initialize_connection(); + //initialize_connection(); + + debug.printf("zaaa\r\n"); + initCaptures(); - + debug.printf("zaaaxx\r\n"); infoTicker.attach(infoTask,0.2f); - if(imu.isReady()) - { + if(imu.isReady()) { debug.printf("MPU9150 is ready\r\n"); - } else - { + } else { debug.printf("MPU9150 initialisation failure\r\n"); } @@ -67,17 +70,14 @@ imu.setDMPEnabled(true); Quaternion q1; - - while(1) - { - if(imu.getFifoCount() >= 48) - { + + while(1) { + if(imu.getFifoCount() >= 48) { imu.getFifoBuffer(buffer, 48); led2 = !led2; } - if(timer.read_ms() > 50) - { + if(timer.read_ms() > 50) { timer.reset(); //This is the format of the data in the fifo, @@ -91,25 +91,25 @@ | 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", (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("%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))); -*/ + 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()); @@ -118,11 +118,66 @@ 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 ",motor2.getRevolutions(),motor2.getPulses()); - // motor1.setVelocity(pry.x/PI*2); + debug.printf("m1: %d m2: %d \r\n ",pulsesMotor1,pulsesMotor2); + motor1.setVelocity(pry.x/PI*2); motor2.setVelocity(pry.x/PI*2); } } - + +} + + + + +void TIMER2_IRQHandler(void) +{ + int bitB = (LPC_GPIO0->FIOPIN>>5) & 0x1; + if(bitB) { + pulsesMotor2++; + } else { + pulsesMotor2--; + } + + LPC_TIM2->IR |= 0x10; +} + +void TIMER0_IRQHandler(void) +{ + int bitB = (LPC_GPIO1->FIOPIN>>27) & 0x1; + if(bitB) { + pulsesMotor1++; + } else { + pulsesMotor1--; + } + + LPC_TIM2->IR |= 0x10; } + +void initCaptures(void) +{ + 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 + LPC_TIM2->TCR = (1 << 0); // start Timer2 + NVIC_EnableIRQ(TIMER2_IRQn); + + LPC_PINCON->PINSEL3 |= 0x00030000; // set P1.26 to CAP0.0 + LPC_SC->PCONP |= (1 << 1); // Timer0 power on + // init Timer 2 (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 + LPC_TIM0->TCR = (1 << 0); // start Timer0 + NVIC_EnableIRQ(TIMER0_IRQn); + +} \ No newline at end of file