Diff: main.cpp

Revision:

22:5cad60d669e6

Parent:

21:c5f88a11eac5

Child:

23:b79faf58d4a1

--- a/main.cpp	Wed May 20 13:04:01 2015 +0000
+++ b/main.cpp	Tue May 26 10:22:47 2015 +0000
@@ -12,6 +12,7 @@
 DigitalOut led3(P2_8);
 AnalogIn hallSensor(P0_26);
 int pulsesMotor2=0,pulsesMotor1=0;
+int int_time=0,bint_time=0;
 //InterruptIn hall1_(P0_5), hall2_(P0_4);
 
 
@@ -86,19 +87,20 @@
     imu.initialiseDMP();
 
     timer.start();
+    timer2.start();
 
     //motor2.setVelocity(0.7);
     imu.setFifoReset(true);
     imu.setDMPEnabled(true);
 
     Quaternion q1;
-    motor1.setAngle(-60);
-    motor2.setAngle(-60);
-    wait_ms(5000);
+    wait_ms(500);
     motor1.setAngle(0);
     
     motor2.setAngle(0);
     initCaptures();
+    motor1.setAngle(0);
+    motor2.setAngle(0);
     
     
     while(1) {
@@ -149,8 +151,9 @@
             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 \r\n ",motor1.get_dt(),motor2.get_dt());
-           /* if(motor1.isRunning()) {
+            debug.printf("dt1: %f dt2: %f ",motor1.get_dt(),motor2.get_dt());
+            debug.printf("Direction: %d Int Time: %d  \r\n ",bint_time,int_time);
+           /* if(!motor1.isRunning()) {
                 if(motor1.getAngle()>30) {
                     motor1.setAngle(0);
                 }
@@ -158,7 +161,7 @@
                     motor1.setAngle(60);
                 }
             }
-            if(motor2.isRunning()) {
+            if(!motor2.isRunning()) {
                 if(motor2.getAngle()>30) {
                     motor2.setAngle(0);
                 }
@@ -214,14 +217,18 @@
 
 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)
@@ -238,6 +245,7 @@
 
 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)
@@ -247,18 +255,20 @@
     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 2 (cap0.0)
+    // 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);