Diff: main.cpp

Revision:

0:5ca44aa85a30

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue Mar 12 21:56:17 2013 +0000
@@ -0,0 +1,226 @@
+#include "ITG3200.h"
+#include "ADXL345_I2C.h"
+
+Serial pc(USBTX, USBRX);
+ITG3200 gyro(p28, p27);
+ADXL345_I2C accel(p28, p27);
+DigitalOut WakeLed(LED1);
+DigitalOut SleepLed(LED2);
+DigitalOut ReadLed(LED3);
+DigitalOut CalLed(LED4);
+
+double w_last_x=0;
+double w_last_y=0;
+double w_last_z=0;
+int autosleep_counter=100;
+
+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);
+void RegPokeAcc(char address, char value);
+void AccCalib();
+void RegdumpAcc();
+
+void sleep1();
+
+InterruptIn pushbutt(p21);
+void bushbutt_handler()
+{ WakeLed=1;
+  SleepLed=0;
+}
+InterruptIn accinterupt(p22);
+void acc_int_handler()
+{ RegPeekAcc(0x30); //clear interrput
+  WakeLed=1;
+  SleepLed=0;
+}
+
+
+int main() {
+
+
+    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()); 
+    
+    // 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;  }
+    //Full resolution, +/-16g, 4mg/LSB.
+    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.setPowerControl(MeasurementMode)) {
+       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();
+    RegPokeAcc(0x24,0x9);//set threshold for activity
+    RegPokeAcc(0x27,0xF0);//Activity axis control
+    RegPokeAcc(0x2E,0x10);//interrupt enable
+    RegPeekAcc(0x30); //clear interrput
+    wait(0.1);
+    pushbutt.rise(&bushbutt_handler); //enable mbed trigger detect
+    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;
+            
+        //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();
+             }   
+        }
+        
+        /*
+        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;
+}
+int16_t double_to_twocomp16(double x)
+{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);
+    return value;
+}
+void RegPokeAcc(char address, char value)
+{
+    accel.SingleByteWrite(address,value);
+}
+
+void AccCalib()
+{   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(0x20,offsetcal((int16_t)accel_read[2]));
+    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=sum0/10/(-4);
+    sum1=sum1/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);
+    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);
+    }    
+//    if(valid==1)
+//    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("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);
+}
+
+void sleep1()
+{
+WakeLed=0;
+SleepLed=1;
+sleep();
+
+}
\ No newline at end of file