Diff: main.cpp

Revision:

5:12e37af16f2e

Parent:

4:cc2b1a6de723

--- a/main.cpp	Wed Mar 14 17:06:53 2012 +0000
+++ b/main.cpp	Mon Sep 03 03:44:13 2012 +0000
@@ -8,6 +8,7 @@
 ITG3200 gyro(p9, p10);
 HMC5883L compass(p9, p10);
 SDFileSystem sd(p5, p6, p7, p8, "sd"); // the pinout on the mbed Cool Components workshop board
+DigitalIn stop(p20);
 Serial pc(USBTX, USBRX);
 
 #define N 3
@@ -31,64 +32,7 @@
     FILE *fp = fopen("/sd/mydir/sdtest.txt", "w");
     if( fp==NULL ){     error("Could not open file for write\n");   }
     
-    // ***  Setting up sensors ***    
-    int preparing_acc();
-    gyro.setLpBandwidth(LPFBW_42HZ);
-    compass.setDefault();
-    wait(0.1);                        // Wait some time for all sensors (Need at least 5ms)
-    
-    pc.printf("\n\rStarting ADXL345, ITG3200 and HMC5883L test...\n\r");
-    pc.printf("   Time     AccX     AccY     AccZ    GyroX    GyroY    GyroZ   MagX   MagY   MagZ\n\r");
-                
-    //while(1){
-    for( int i=0;i<10000;i++ ){
-    
-        // Updating accelerometer and compass
-        accelerometer.getOutput(bitAcc);
-        compass.readData(getMag);
-        
-        // Transfering units (Acc[g], Gyro[deg/s], Compass[Ga])
-        // Calibration YAL 9DOF Acc:X+6, Y-12, Z+44
-        // Calibration green Acc:X+1, Y-18, Z+45
-        Acc[0]  =  ((int16_t)bitAcc[0])*0.004;
-        Acc[1]  =  ((int16_t)bitAcc[1])*0.004;
-        Acc[2]  =  ((int16_t)bitAcc[2])*0.004;
-        // Calibration YAL 9DOF Gyro:X+28, Y-45, Z+34 
-        // Calibration green Gyro:X+135, Y-12, Z-53 
-        Gyro[0] =  (gyro.getGyroX())/14.375;
-        Gyro[1] =  (gyro.getGyroY())/14.375;
-        Gyro[2] =  (gyro.getGyroZ())/14.375;
-        // Calibration YAL 9DOF Compass:X, Y, Z
-        Mag[0]  =  (int16_t)getMag[0];
-        Mag[1]  =  (int16_t)getMag[1];
-        Mag[2]  =  (int16_t)getMag[2];
-        
-        // Low pass filter for acc
-        //for ( int i=0;i<N;i++ ){
-        //    if( -0.05<Acc[i] && Acc[i]<0.05 ){    Acc[i]=0;  }
-        //}
-        
-        // Low pass filter for gyro
-        //for ( int i=0;i<N;i++ ){
-        //    if( -1.0<Gyro[i] && Gyro[i]<1.0 ){    Gyro[i]=0;  }
-        //}
-
-        //pc.printf("%7.2f, %7.3f, %7.3f, %7.3f, %7.1f, %7.1f, %7.1f, %5d, %5d, %5d\n\r", t, Acc[0], Acc[1], Acc[2], Gyro[0], Gyro[1], Gyro[2], Mag[0], Mag[1], Mag[2]);
-        pc.printf("%7.2f, %7.3f, %7.3f, %7.3f, %7.1f, %7.1f, %7.1f\n\r", t, Acc[0], Acc[1], Acc[2], Gyro[0], Gyro[1], Gyro[2]);
-        //pc.printf("%7.1f, %7.1f, %7.1f\n\r",Gyro[0], Gyro[1], Gyro[2]);
-        fprintf(fp, "%7.2f, %7.3f, %7.3f, %7.3f, %7.1f, %7.1f, %7.1f\n\r", t, Acc[0], Acc[1], Acc[2], Gyro[0], Gyro[1], Gyro[2]);
-        
-        t += dt;
-        wait(dt);
-        
-    }
-    
-    fclose(fp);
-    
- }
- 
-int preparing_acc(){
- 
+    // *** start up ***
     // These are here to test whether any of the initialization fails. It will print the failure.
     if (accelerometer.setPowerControl(0x00)){
         pc.printf("didn't intitialize power control\n\r"); 
@@ -113,8 +57,57 @@
     if(accelerometer.setPowerControl(MeasurementMode)) {
         pc.printf("didn't set the power control to measurement\n\r"); 
         return 0;
-    } 
- 
-    return 0;
- 
-}
+    }
+
+    gyro.setLpBandwidth(LPFBW_42HZ);
+    compass.setDefault();
+    wait(0.1);                        // Wait some time for all sensors (Need at least 5ms)
+    // *** start up ***
+        
+    pc.printf("\n\rStarting ADXL345, ITG3200 and HMC5883L test...\n\r");
+    pc.printf("   Time     AccX     AccY     AccZ    GyroX    GyroY    GyroZ   MagX   MagY   MagZ\n\r");
+
+    while( stop==0 ){
+    //for( int i=0;i<10000;i++ ){
+    
+        // Updating accelerometer and compass
+        accelerometer.getOutput(bitAcc);
+        compass.readData(getMag);
+        
+        // Transfering units (Acc[g], Gyro[deg/s], Compass[Ga])
+        Acc[0]  =  ((int16_t)bitAcc[0])*0.004;
+        Acc[1]  =  ((int16_t)bitAcc[1])*0.004;
+        Acc[2]  =  ((int16_t)bitAcc[2])*0.004;
+
+        Gyro[0] =  (gyro.getGyroX())/14.375;
+        Gyro[1] =  (gyro.getGyroY())/14.375;
+        Gyro[2] =  (gyro.getGyroZ())/14.375;
+        // Calibration YAL 9DOF Compass:X, Y, Z
+        Mag[0]  =  (int16_t)getMag[0];
+        Mag[1]  =  (int16_t)getMag[1];
+        Mag[2]  =  (int16_t)getMag[2];
+        
+        // Low pass filter for acc
+        //for ( int i=0;i<N;i++ ){
+        //    if( -0.05<Acc[i] && Acc[i]<0.05 ){    Acc[i]=0;  }
+        //}
+        
+        // Low pass filter for gyro
+        //for ( int i=0;i<N;i++ ){
+        //    if( -1.0<Gyro[i] && Gyro[i]<1.0 ){    Gyro[i]=0;  }
+        //}
+
+        //pc.printf("%7.2f, %7.3f, %7.3f, %7.3f, %7.1f, %7.1f, %7.1f, %5d, %5d, %5d\n\r", t, Acc[0], Acc[1], Acc[2], Gyro[0], Gyro[1], Gyro[2], Mag[0], Mag[1], Mag[2]);
+        pc.printf("%7.2f, %7.3f, %7.3f, %7.3f, %7.1f, %7.1f, %7.1f\n\r", t, Acc[0], Acc[1], Acc[2], Gyro[0], Gyro[1], Gyro[2]);
+        //pc.printf("%7.1f, %7.1f, %7.1f\n\r",Gyro[0], Gyro[1], Gyro[2]);
+        fprintf(fp, "%7.2f, %7.3f, %7.3f, %7.3f, %7.1f, %7.1f, %7.1f\n", t, Acc[0], Acc[1], Acc[2], Gyro[0], Gyro[1], Gyro[2]);
+        
+        t += dt;
+        wait(dt);
+        
+    }
+    
+    fclose(fp);
+    
+ }
+ 
\ No newline at end of file