Diff: main.cpp

Revision:

10:09dbd00164b9

Parent:

9:ca52462f9ebc

Child:

11:ba1a630e56c4

--- a/main.cpp	Tue May 05 09:07:11 2015 +0000
+++ b/main.cpp	Wed May 13 11:55:50 2015 +0000
@@ -1,11 +1,24 @@
 #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);
@@ -26,15 +39,34 @@
 {
     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();
 
-   // initialize_connection();
-    
     infoTicker.attach(infoTask,0.2f);
 
     if(imu.isReady()) {
@@ -57,7 +89,7 @@
         if(imu.getFifoCount() >= 48) {
             imu.getFifoBuffer(buffer,  48);
             led2 = !led2;
-            debug.printf("%d\r\n",timer2.read_ms());
+            //debug.printf("%d\r\n",timer2.read_ms());
             timer2.reset();
         }
 
@@ -96,32 +128,106 @@
             */
 
             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();
-            /*Vector3 vector_x_local=q1.rotate(vector_x);
-            Vector3 vector_y_local=q1.rotate(vector_y);
-            Vector3 vector_z_local=q1.rotate(vector_z);
-            Vector3 direction_plane_normal=vector_z_local.cross_product(vector_z);
-            Vector3 bar_projection=vector_z-vector_x_local*(vector_z.dot_product(vector_x_local)/vector_x_local.length_squared());
-            Vector3 bar_projection_g=bar_projection-vector_z*(bar_projection.dot_product(vector_z)/vector_z.length_squared());
-            float angle=acos(vector_y_local.dot_product(vector_z)/vector_y_local.length()/vector_z.length());*/
-            //debug.printf("angle: %f  ", angle/PI*180);
-            debug.printf("p: %f r: %f y: %f \r\n ",pry.x/PI*180,pry.y/PI*180,pry.z/PI*180);
+            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);*/
 
-            //TeaPot Demo Packet for MotionFit SDK
-            /*
-            debug.putc('$'); //packet start
-            debug.putc((char)2); //assume packet type constant
-            debug.putc((char)0); //count seems unused
-            for(int i = 0; i < 16; i++){ //16 bytes for 4 32bit floats
-                debug.putc((char)(buffer[i]));
-            }
-            for(int i = 0; i < 5; i++){ //no idea, padded with 0
-                debug.putc((char)0);
-            }
-            */
         }
     }
 }
+
+/*/////////////////////////////////////////////////////////////////////////////////////////////////////////
+//  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;
+}*/
\ No newline at end of file