A system demonstrating interface of 6DOF sensor to Android
Fork of 6dof_new_workwith_v2 by
Diff: main.cpp
- Revision:
- 0:5ca44aa85a30
- Child:
- 1:6ec19e5615d9
diff -r 000000000000 -r 5ca44aa85a30 main.cpp --- /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