Diff: main.cpp

Revision:

17:b3acd6416356

Parent:

15:55b8bfa4d09c

Child:

18:9f249b8a59ae

--- a/main.cpp	Fri May 15 08:13:56 2015 +0000
+++ b/main.cpp	Sat May 16 08:50:02 2015 +0000
@@ -9,10 +9,7 @@
 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);
-
-Motor motor1(P2_0, P2_1,&hall1);
+//InterruptIn hall1_(P0_5), hall2_(P0_4);
 
 
 Serial RN42(P0_10, P0_11);
@@ -24,11 +21,15 @@
 Timer timer2;
 
 
+
+MPU9150 imu(P0_28, P0_27, P2_13);
+
 Hallsensor hall2(P0_4, P0_5);
 Motor motor2(P2_2, P2_3, &hall2);
 
-MPU9150 imu(P0_28, P0_27, P2_13);
+//Hallsensor hall1(P1_26, P1_27);
 
+//Motor motor1(P2_0, P2_1,&hall1);
 char buffer[200];
 void infoTask(void)
 {
@@ -49,30 +50,30 @@
     
     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");
     }
 
     imu.initialiseDMP();
 
     timer.start();
-    timer2.start();
 
-            motor2.setVelocity(0.7);
+    //motor2.setVelocity(0.7);
     imu.setFifoReset(true);
     imu.setDMPEnabled(true);
 
     Quaternion q1;
+    
     while(1) 
     {
-
-        if(imu.getFifoCount() >= 48) {
+        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)
@@ -90,8 +91,8 @@
               |  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]),
+            
+          /*  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]));
 
@@ -108,8 +109,7 @@
                 (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,8 +118,9 @@
             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);
+            debug.printf("m1: %d m2: %d \r\n ",motor2.getRevolutions(),motor2.getPulses());
+           // motor1.setVelocity(pry.x/PI*2);
+            motor2.setVelocity(pry.x/PI*2);
 
         }
     }