Diff: main.cpp

Revision:

13:75892580d4ff

Parent:

12:410ebe8573ce

Child:

14:94a02b617085

--- a/main.cpp	Thu May 14 09:04:28 2015 +0000
+++ b/main.cpp	Thu May 14 13:32:39 2015 +0000
@@ -10,10 +10,10 @@
 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);
@@ -28,7 +28,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)
@@ -50,31 +50,31 @@
     
     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();
 
-    imu.setFifoReset(true);
-    imu.setDMPEnabled(true);
+  //  imu.setFifoReset(true);
+   // imu.setDMPEnabled(true);
 
-    Quaternion q1;
+ //   Quaternion q1;
     while(1) {
 
-        if(imu.getFifoCount() >= 48) {
-            imu.getFifoBuffer(buffer,  48);
-            led2 = !led2;
+     //   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) {
+    /*    if(timer.read_ms() > 50) {
             timer.reset();
 
             //This is the format of the data in the fifo,
@@ -108,18 +108,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());
+       //     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);
             motor2.setVelocity(0.7);
 
         }
-    }
-}
+    
+