Diff: main.cpp

Revision:

15:55b8bfa4d09c

Parent:

14:94a02b617085

Child:

17:b3acd6416356

--- a/main.cpp	Thu May 14 15:13:13 2015 +0000
+++ b/main.cpp	Fri May 15 08:10:06 2015 +0000
@@ -4,19 +4,18 @@
 #include "Motor.h"
 
 
-
 //led pins
 DigitalOut led1(P2_6);
 DigitalOut led2(P2_7);
 DigitalOut led3(P2_8);
 AnalogIn hallSensor(P0_26);
-//InterruptIn hall1_(P0_5), hall2_(P0_4);
-//Hallsensor hall1(P1_26, P1_27);
+InterruptIn hall1_(P0_5), hall2_(P0_4);
+Hallsensor hall1(P1_26, P1_27);
 
-//Motor motor1(P2_0, P2_1,&hall1);
+Motor motor1(P2_0, P2_1,&hall1);
 
 
-//Serial RN42(P0_10, P0_11);
+Serial RN42(P0_10, P0_11);
 Serial debug(P0_2, P0_3);
 
 Ticker infoTicker;
@@ -28,7 +27,7 @@
 Hallsensor hall2(P0_4, P0_5);
 Motor motor2(P2_2, P2_3, &hall2);
 
-//MPU9150 imu(P0_28, P0_27, P2_13);
+MPU9150 imu(P0_28, P0_27, P2_13);
 
 char buffer[200];
 void infoTask(void)
@@ -42,41 +41,43 @@
 int main()
 {
    
-  //  RN42.baud(9600);
+    RN42.baud(9600);
     debug.baud(115200);
-   // InitTimer0();
-    // initialize_connection();
+    //InitTimer0();
+     //initialize_connection();
 
     
     infoTicker.attach(infoTask,0.2f);
 
-  /*  if(imu.isReady()) {
+    if(imu.isReady()) {
         debug.printf("MPU9150 is ready\r\n");
     } else {
         debug.printf("MPU9150 initialisation failure\r\n");
     }
-*/
-   // imu.initialiseDMP();
+
+    imu.initialiseDMP();
 
     timer.start();
     timer2.start();
 
             motor2.setVelocity(0.7);
-  //  imu.setFifoReset(true);
-   // imu.setDMPEnabled(true);
+    imu.setFifoReset(true);
+    imu.setDMPEnabled(true);
 
- //   Quaternion q1;
-    while(1) {
+    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(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();
+        if(timer.read_ms() > 50)
+        {
+            timer.reset();
 
             //This is the format of the data in the fifo,
             /* ================================================================================================ *
@@ -109,17 +110,18 @@
                 (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);
+            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);
+            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 ",motor2.getPulses(),motor2.getPulses());
-            //motor1.setVelocity(pry.x/90);
+            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.getPulses(),motor2.getPulses());
+            motor1.setVelocity(pry.x/90);
 
         }
+    }
     
 }