Revision 1:6ec19e5615d9, committed 2013-03-18

Comitter:

Vigneshwar

Date:

Mon Mar 18 02:32:48 2013 +0000

Parent:

0:5ca44aa85a30

Commit message:

Micropower Wireless Health sensors

Changed in this revision

diff -r 5ca44aa85a30 -r 6ec19e5615d9 main.cpp
--- a/main.cpp	Tue Mar 12 21:56:17 2013 +0000
+++ b/main.cpp	Mon Mar 18 02:32:48 2013 +0000
@@ -1,7 +1,8 @@
 #include "ITG3200.h"
 #include "ADXL345_I2C.h"
 
-Serial pc(USBTX, USBRX);
+Serial rn42(p9, p10); 
+//Serial pc(USBTX, USBRX);
 ITG3200 gyro(p28, p27);
 ADXL345_I2C accel(p28, p27);
 DigitalOut WakeLed(LED1);
@@ -14,7 +15,7 @@
 double w_last_z=0;
 int autosleep_counter=100;
 
-int accel_read[3] = {0, 0, 0};  
+int accel_read[3] = {0, 0, 0};
 double twocomp16_to_double(int16_t x);
 int16_t double_to_twocomp16(double x);
 char RegPeekAcc(char address);
@@ -26,50 +27,57 @@
 
 InterruptIn pushbutt(p21);
 void bushbutt_handler()
-{ WakeLed=1;
-  SleepLed=0;
+{
+    WakeLed=1;
+    SleepLed=0;
 }
+
 InterruptIn accinterupt(p22);
 void acc_int_handler()
-{ RegPeekAcc(0x30); //clear interrput
-  WakeLed=1;
-  SleepLed=0;
+{
+    RegPeekAcc(0x30); //clear interrput
+    WakeLed=1;
+    SleepLed=0;
 }
 
 
-int main() {
+int main()
+{
 
-
+    rn42.baud(115200);
     char str[512];
     gyro.setWhoAmI(0x68);
-    pc.printf("Now starting 6-degrees-of-freedom (ITG-3200 ADXL345) test...\n");
-    pc.printf("Accelerometer Device ID is: 0x%02x\n", accel.getDeviceID());
-    pc.printf("Gyro Devide ID is: 0x%02x\n", gyro.getWhoAmI()); 
-    
+    //pc.printf("Now starting 6-degrees-of-freedom (ITG-3200 ADXL345) test...\n");
+    //pc.printf("Accelerometer Device ID is: 0x%02x\n", accel.getDeviceID());
+    //pc.printf("Gyro Devide ID is: 0x%02x\n", gyro.getWhoAmI());
+
     // Accel setup
     // These are here to test whether any of the initialization fails. It will print the failure
-    if (accel.setPowerControl(0x00)){
-         pc.printf("didn't intitialize power control\n"); 
-         return 0;  }
+    if (accel.setPowerControl(0x00)) {
+        //pc.printf("didn't intitialize power control\n");
+        return 0;
+    }
     //Full resolution, +/-16g, 4mg/LSB.
-    wait(.001);    
-    if(accel.setDataFormatControl(0x0B)){
-        pc.printf("didn't set data format\n");
-        return 0;  }
+    wait(.001);
+    if(accel.setDataFormatControl(0x0B)) {
+        //pc.printf("didn't set data format\n");
+        return 0;
+    }
     wait(.001);
     //3.2kHz data rate.
-    if(accel.setDataRate(ADXL345_3200HZ)){
-       pc.printf("didn't set data rate\n");
-       return 0;    }
-    wait(.001);   
-    //Measurement mode.    
+    if(accel.setDataRate(ADXL345_3200HZ)) {
+        //pc.printf("didn't set data rate\n");
+        return 0;
+    }
+    wait(.001);
+    //Measurement mode.
     if(accel.setPowerControl(MeasurementMode)) {
-       pc.printf("didn't set the power control to measurement\n"); 
-       return 0;   
-    }     
-    // Gyro setup   
+        //pc.printf("didn't set the power control to measurement\n");
+        return 0;
+    }
+    // Gyro setup
     gyro.setLpBandwidth(LPFBW_42HZ);
-       
+
     //Added for Acc
     WakeLed=1;
     AccCalib();
@@ -79,76 +87,86 @@
     RegPeekAcc(0x30); //clear interrput
     wait(0.1);
     pushbutt.rise(&bushbutt_handler); //enable mbed trigger detect
-    accinterupt.rise(&acc_int_handler);  
+    accinterupt.rise(&acc_int_handler);
     RegdumpAcc();
 
-    
+
     if(true)
-    for(int i=0;i<=100000;i++)
-    {
-        wait(0.01);
-        
-        if(ReadLed==1) ReadLed=0;
-        else ReadLed=1;
-        
-        accel.getOutput(accel_read);
-        
-        sprintf(str, "%i,%i,%i,%i,%i,%i,%i\n\r", (int16_t)accel_read[0], (int16_t)accel_read[1], (int16_t)accel_read[2],
-                                                     gyro.getGyroX(), gyro.getGyroY(), gyro.getGyroZ(), (int16_t)gyro.getTemperature());  
-    
-        double d_accelx=(double) (int16_t)accel_read[0];
-        double d_accely=(double) (int16_t)accel_read[1];
-        double d_accelz=(double) (int16_t)accel_read[2];  
-        double wp=6.28e-2; //IIR pole for DC cancellation
-        double w_x,w_y,w_z;
-        double dc_x,dc_y,dc_z;
+        for(int i=0; i<=100000; i++) {
+            wait(0.01);
+
+            if(ReadLed==1) ReadLed=0;
+            else ReadLed=1;
+
+            accel.getOutput(accel_read);
+
+            sprintf(str, "%i,%i,%i,%i,%i,%i,%i,A", (int16_t)accel_read[0], (int16_t)accel_read[1], (int16_t)accel_read[2],
+                    gyro.getGyroX(), gyro.getGyroY(), gyro.getGyroZ(), (int16_t)gyro.getTemperature());
+            
+            rn42.printf(str);
             
-        //IIR calculate DC term in acceleration
-        w_x=d_accelx+ (1-wp)*w_last_x; dc_x=wp*w_x; w_last_x=w_x;     
-        w_y=d_accely+ (1-wp)*w_last_y; dc_y=wp*w_y; w_last_y=w_y;  
-        w_z=d_accelz+ (1-wp)*w_last_z; dc_z=wp*w_z; w_last_z=w_z;   
-      
-        d_accelx=d_accelx-dc_x;
-        d_accely=d_accely-dc_y;
-        d_accelz=d_accelz-dc_z;            
-        double mag_accel=d_accelx*d_accelx+d_accely*d_accely+d_accelz*d_accelz;
-        mag_accel=sqrt(mag_accel);
-        pc.printf("With DC Cal:Double %f %f %f . Offset is %f %f %f. Mag %f\n\r",d_accelx,d_accely,d_accelz,dc_x,dc_y,dc_z,mag_accel);       
-        RegPeekAcc(0x30);
-        
-        if(autosleep_counter>0) autosleep_counter-=1;
-        else
-        { if(mag_accel<15) 
-             {sleep1();
-             }   
+            double d_accelx=(double) (int16_t)accel_read[0];
+            double d_accely=(double) (int16_t)accel_read[1];
+            double d_accelz=(double) (int16_t)accel_read[2];
+            double wp=6.28e-2; //IIR pole for DC cancellation
+            double w_x,w_y,w_z;
+            double dc_x,dc_y,dc_z;
+
+            //IIR calculate DC term in acceleration
+            w_x=d_accelx+ (1-wp)*w_last_x;
+            dc_x=wp*w_x;
+            w_last_x=w_x;
+            w_y=d_accely+ (1-wp)*w_last_y;
+            dc_y=wp*w_y;
+            w_last_y=w_y;
+            w_z=d_accelz+ (1-wp)*w_last_z;
+            dc_z=wp*w_z;
+            w_last_z=w_z;
+
+            d_accelx=d_accelx-dc_x;
+            d_accely=d_accely-dc_y;
+            d_accelz=d_accelz-dc_z;
+            double mag_accel=d_accelx*d_accelx+d_accely*d_accely+d_accelz*d_accelz;
+            mag_accel=sqrt(mag_accel);
+            //pc.printf("With DC Cal:Double %f %f %f . Offset is %f %f %f. Mag %f\n\r",d_accelx,d_accely,d_accelz,dc_x,dc_y,dc_z,mag_accel);
+            RegPeekAcc(0x30);
+
+            if(autosleep_counter>0) autosleep_counter-=1;
+            else {
+                if(mag_accel<15) {
+//                    sleep1();
+                    autosleep_counter=30;
+                }
+            }
+
+            /*
+            pc.printf("Gyro[%5i, %5i, %5i]   Accel[%4i, %4i, %4i]\n\t",
+                                gyro.getGyroX(), gyro.getGyroY(), gyro.getGyroZ(),
+                                (int16_t)accel_read[0], (int16_t)accel_read[1], (int16_t)accel_read[2]);
+            */
         }
-        
-        /*
-        pc.printf("Gyro[%5i, %5i, %5i]   Accel[%4i, %4i, %4i]\n\t", 
-                            gyro.getGyroX(), gyro.getGyroY(), gyro.getGyroZ(),
-                            (int16_t)accel_read[0], (int16_t)accel_read[1], (int16_t)accel_read[2]);
-        */
-    }
 
 
 }
 
 
 double twocomp16_to_double(int16_t x)
-{double a;
-a= double(x);
-return a;
+{
+    double a;
+    a= double(x);
+    return a;
 }
 int16_t double_to_twocomp16(double x)
-{int16_t a;
- a=(signed int16_t) x;
- return a;
+{
+    int16_t a;
+    a=(signed int16_t) x;
+    return a;
 }
 
 char RegPeekAcc(char address)
 {
     char value=accel.SingleByteRead(address);
-    pc.printf("AccReg address: %X value: %X\n\r",address,value);
+    //pc.printf("AccReg address: %X value: %X\n\r",address,value);
     return value;
 }
 void RegPokeAcc(char address, char value)
@@ -157,70 +175,87 @@
 }
 
 void AccCalib()
-{   CalLed=1;
-    double acc0=0,acc1=0,acc2=0; 
-    double sum0=0,sum1=0,sum2=0; 
+{
+    CalLed=1;
+    double acc0=0,acc1=0,acc2=0;
+    double sum0=0,sum1=0,sum2=0;
     wait(0.1) ;
     accel.getOutput(accel_read);
-//    RegPokeAcc(0x1e,offsetcal((int16_t)accel_read[0])); 
-//    RegPokeAcc(0x1f,offsetcal((int16_t)accel_read[1])); 
+//    RegPokeAcc(0x1e,offsetcal((int16_t)accel_read[0]));
+//    RegPokeAcc(0x1f,offsetcal((int16_t)accel_read[1]));
 //    RegPokeAcc(0x20,offsetcal((int16_t)accel_read[2]));
-    for(int n=1;n<=10;n++){
+    for(int n=1; n<=10; n++) {
         wait(0.1);
         accel.getOutput(accel_read);
         acc0=(double)(int16_t)accel_read[0];
         acc1=(double)(int16_t)accel_read[1];
         acc2=(double)(int16_t)accel_read[2];
-        sum0+=acc0;sum1+=acc1;sum2+=acc2;
+        sum0+=acc0;
+        sum1+=acc1;
+        sum2+=acc2;
     }
     sum0=sum0/10/(-4);
     sum1=sum1/10/(-4);
-    sum2=sum2/10/(-4);    
+    sum2=sum2/10/(-4);
     int16_t acc0_16=(signed int16_t)sum0;
     int16_t acc1_16=(signed int16_t)sum1;
-    int16_t acc2_16=(signed int16_t)sum2;    
-    RegPokeAcc(0x1e,acc0_16); 
-    RegPokeAcc(0x1f,acc1_16); 
-    RegPokeAcc(0x20,acc2_16); 
- //   pc.printf("Calibration. Calculated offset %i   %i   %i\n\r",acc0_16,acc1_16,acc2_16);
+    int16_t acc2_16=(signed int16_t)sum2;
+    RegPokeAcc(0x1e,acc0_16);
+    RegPokeAcc(0x1f,acc1_16);
+    RegPokeAcc(0x20,acc2_16);
+//   pc.printf("Calibration. Calculated offset %i   %i   %i\n\r",acc0_16,acc1_16,acc2_16);
     int valid=1;
-    for(int n=1;n<=10;n++)
-    {wait(0.1);
-    accel.getOutput(accel_read);
-    accel.getOutput(accel_read);
-    acc0=(double)(int16_t)accel_read[0]; if(abs(acc0)>50) valid=0;
-    acc1=(double)(int16_t)accel_read[1];if(abs(acc1)>50) valid=0;
-    acc2=(double)(int16_t)accel_read[2];if(abs(acc2)>50) valid=0;
- //   pc.printf("verifying cal %i,%i,%i \n\r", (int16_t)accel_read[0], (int16_t)accel_read[1], (int16_t)accel_read[2]);
- //   pc.printf("verifying cal (double)%f %f %f\n\r",acc0,acc1,acc2);
- //   pc.printf("valid= %d \n\r",valid);
-    }    
+    for(int n=1; n<=10; n++) {
+        wait(0.1);
+        accel.getOutput(accel_read);
+        accel.getOutput(accel_read);
+        acc0=(double)(int16_t)accel_read[0];
+        if(abs(acc0)>50) valid=0;
+        acc1=(double)(int16_t)accel_read[1];
+        if(abs(acc1)>50) valid=0;
+        acc2=(double)(int16_t)accel_read[2];
+        if(abs(acc2)>50) valid=0;
+//   pc.printf("verifying cal %i,%i,%i \n\r", (int16_t)accel_read[0], (int16_t)accel_read[1], (int16_t)accel_read[2]);
+//   pc.printf("verifying cal (double)%f %f %f\n\r",acc0,acc1,acc2);
+//   pc.printf("valid= %d \n\r",valid);
+    }
 //    if(valid==1)
-//    CalLed=0;
+    CalLed=0;
 //    else AccCalib();
 }
 
 void RegdumpAcc()
 {
-pc.printf("\r---------a new regdump start\n\r-------");
-pc.printf("Acc DeviceID \t");RegPeekAcc(0x00);
-pc.printf("X_asis: \t");RegPeekAcc(0x1e);
-pc.printf("Y_asis: \t");RegPeekAcc(0x1f);
-pc.printf("Z_asis: \t");RegPeekAcc(0x20);
-pc.printf("Act Thresh: \t");RegPeekAcc(0x24);
-pc.printf("Axis Enable: \t");RegPeekAcc(0x27);
-pc.printf("Interpt Ctrl: \t");RegPeekAcc(0x2e);
-pc.printf("Interpt Mapping: \t");RegPeekAcc(0x2f);
+    //pc.printf("\r---------a new regdump start\n\r-------");
+    //pc.printf("Acc DeviceID \t");
+    RegPeekAcc(0x00);
+    //pc.printf("X_asis: \t");
+    RegPeekAcc(0x1e);
+    //pc.printf("Y_asis: \t");
+    RegPeekAcc(0x1f);
+    //pc.printf("Z_asis: \t");
+    RegPeekAcc(0x20);
+    //pc.printf("Act Thresh: \t");
+    RegPeekAcc(0x24);
+    //pc.printf("Axis Enable: \t");
+    RegPeekAcc(0x27);
+    //pc.printf("Interpt Ctrl: \t");
+    RegPeekAcc(0x2e);
+    //pc.printf("Interpt Mapping: \t");
+    RegPeekAcc(0x2f);
 //pc.printf("Source of interupt:\t");RegPeekAcc(0x30);
-pc.printf("Data format control\t");RegPeekAcc(0x31);
-pc.printf("FIFO Ctrl:\t");RegPeekAcc(0x38);
-pc.printf("FIFO Status:\t");RegPeekAcc(0x39);
+    //pc.printf("Data format control\t");
+    RegPeekAcc(0x31);
+    //pc.printf("FIFO Ctrl:\t");
+    RegPeekAcc(0x38);
+    //pc.printf("FIFO Status:\t");
+    RegPeekAcc(0x39);
 }
 
 void sleep1()
 {
-WakeLed=0;
-SleepLed=1;
-sleep();
+    WakeLed=0;
+    SleepLed=1;
+    sleep();
 
 }
\ No newline at end of file