Vigneshwar Murali / Mbed 2 deprecated 6dof_new_workwith_v2

A system demonstrating interface of 6DOF sensor to Android

Dependencies:   ITG3200 mbed

Fork of 6dof_new_workwith_v2 by Chen Zhai

main.cpp@1:6ec19e5615d9, 2013-03-18 (annotated)

Committer:
Vigneshwar
Date:
Mon Mar 18 02:32:48 2013 +0000
Revision:
1:6ec19e5615d9
Parent:
0:5ca44aa85a30 
Micropower Wireless Health sensors

Who changed what in which revision?

UserRevisionLine numberNew contents of line
chenzhai 0:5ca44aa85a30 1 #include "ITG3200.h"
chenzhai 0:5ca44aa85a30 2 #include "ADXL345_I2C.h"
chenzhai 0:5ca44aa85a30 3
Vigneshwar 1:6ec19e5615d9 4 Serial rn42(p9, p10);
Vigneshwar 1:6ec19e5615d9 5 //Serial pc(USBTX, USBRX);
chenzhai 0:5ca44aa85a30 6 ITG3200 gyro(p28, p27);
chenzhai 0:5ca44aa85a30 7 ADXL345_I2C accel(p28, p27);
chenzhai 0:5ca44aa85a30 8 DigitalOut WakeLed(LED1);
chenzhai 0:5ca44aa85a30 9 DigitalOut SleepLed(LED2);
chenzhai 0:5ca44aa85a30 10 DigitalOut ReadLed(LED3);
chenzhai 0:5ca44aa85a30 11 DigitalOut CalLed(LED4);
chenzhai 0:5ca44aa85a30 12
chenzhai 0:5ca44aa85a30 13 double w_last_x=0;
chenzhai 0:5ca44aa85a30 14 double w_last_y=0;
chenzhai 0:5ca44aa85a30 15 double w_last_z=0;
chenzhai 0:5ca44aa85a30 16 int autosleep_counter=100;
chenzhai 0:5ca44aa85a30 17
Vigneshwar 1:6ec19e5615d9 18 int accel_read[3] = {0, 0, 0};
chenzhai 0:5ca44aa85a30 19 double twocomp16_to_double(int16_t x);
chenzhai 0:5ca44aa85a30 20 int16_t double_to_twocomp16(double x);
chenzhai 0:5ca44aa85a30 21 char RegPeekAcc(char address);
chenzhai 0:5ca44aa85a30 22 void RegPokeAcc(char address, char value);
chenzhai 0:5ca44aa85a30 23 void AccCalib();
chenzhai 0:5ca44aa85a30 24 void RegdumpAcc();
chenzhai 0:5ca44aa85a30 25
chenzhai 0:5ca44aa85a30 26 void sleep1();
chenzhai 0:5ca44aa85a30 27
chenzhai 0:5ca44aa85a30 28 InterruptIn pushbutt(p21);
chenzhai 0:5ca44aa85a30 29 void bushbutt_handler()
Vigneshwar 1:6ec19e5615d9 30 {
Vigneshwar 1:6ec19e5615d9 31 WakeLed=1;
Vigneshwar 1:6ec19e5615d9 32 SleepLed=0;
chenzhai 0:5ca44aa85a30 33 }
Vigneshwar 1:6ec19e5615d9 34
chenzhai 0:5ca44aa85a30 35 InterruptIn accinterupt(p22);
chenzhai 0:5ca44aa85a30 36 void acc_int_handler()
Vigneshwar 1:6ec19e5615d9 37 {
Vigneshwar 1:6ec19e5615d9 38 RegPeekAcc(0x30); //clear interrput
Vigneshwar 1:6ec19e5615d9 39 WakeLed=1;
Vigneshwar 1:6ec19e5615d9 40 SleepLed=0;
chenzhai 0:5ca44aa85a30 41 }
chenzhai 0:5ca44aa85a30 42
chenzhai 0:5ca44aa85a30 43
Vigneshwar 1:6ec19e5615d9 44 int main()
Vigneshwar 1:6ec19e5615d9 45 {
chenzhai 0:5ca44aa85a30 46
Vigneshwar 1:6ec19e5615d9 47 rn42.baud(115200);
chenzhai 0:5ca44aa85a30 48 char str[512];
chenzhai 0:5ca44aa85a30 49 gyro.setWhoAmI(0x68);
Vigneshwar 1:6ec19e5615d9 50 //pc.printf("Now starting 6-degrees-of-freedom (ITG-3200 ADXL345) test...\n");
Vigneshwar 1:6ec19e5615d9 51 //pc.printf("Accelerometer Device ID is: 0x%02x\n", accel.getDeviceID());
Vigneshwar 1:6ec19e5615d9 52 //pc.printf("Gyro Devide ID is: 0x%02x\n", gyro.getWhoAmI());
Vigneshwar 1:6ec19e5615d9 53
chenzhai 0:5ca44aa85a30 54 // Accel setup
chenzhai 0:5ca44aa85a30 55 // These are here to test whether any of the initialization fails. It will print the failure
Vigneshwar 1:6ec19e5615d9 56 if (accel.setPowerControl(0x00)) {
Vigneshwar 1:6ec19e5615d9 57 //pc.printf("didn't intitialize power control\n");
Vigneshwar 1:6ec19e5615d9 58 return 0;
Vigneshwar 1:6ec19e5615d9 59 }
chenzhai 0:5ca44aa85a30 60 //Full resolution, +/-16g, 4mg/LSB.
Vigneshwar 1:6ec19e5615d9 61 wait(.001);
Vigneshwar 1:6ec19e5615d9 62 if(accel.setDataFormatControl(0x0B)) {
Vigneshwar 1:6ec19e5615d9 63 //pc.printf("didn't set data format\n");
Vigneshwar 1:6ec19e5615d9 64 return 0;
Vigneshwar 1:6ec19e5615d9 65 }
chenzhai 0:5ca44aa85a30 66 wait(.001);
chenzhai 0:5ca44aa85a30 67 //3.2kHz data rate.
Vigneshwar 1:6ec19e5615d9 68 if(accel.setDataRate(ADXL345_3200HZ)) {
Vigneshwar 1:6ec19e5615d9 69 //pc.printf("didn't set data rate\n");
Vigneshwar 1:6ec19e5615d9 70 return 0;
Vigneshwar 1:6ec19e5615d9 71 }
Vigneshwar 1:6ec19e5615d9 72 wait(.001);
Vigneshwar 1:6ec19e5615d9 73 //Measurement mode.
chenzhai 0:5ca44aa85a30 74 if(accel.setPowerControl(MeasurementMode)) {
Vigneshwar 1:6ec19e5615d9 75 //pc.printf("didn't set the power control to measurement\n");
Vigneshwar 1:6ec19e5615d9 76 return 0;
Vigneshwar 1:6ec19e5615d9 77 }
Vigneshwar 1:6ec19e5615d9 78 // Gyro setup
chenzhai 0:5ca44aa85a30 79 gyro.setLpBandwidth(LPFBW_42HZ);
Vigneshwar 1:6ec19e5615d9 80
chenzhai 0:5ca44aa85a30 81 //Added for Acc
chenzhai 0:5ca44aa85a30 82 WakeLed=1;
chenzhai 0:5ca44aa85a30 83 AccCalib();
chenzhai 0:5ca44aa85a30 84 RegPokeAcc(0x24,0x9);//set threshold for activity
chenzhai 0:5ca44aa85a30 85 RegPokeAcc(0x27,0xF0);//Activity axis control
chenzhai 0:5ca44aa85a30 86 RegPokeAcc(0x2E,0x10);//interrupt enable
chenzhai 0:5ca44aa85a30 87 RegPeekAcc(0x30); //clear interrput
chenzhai 0:5ca44aa85a30 88 wait(0.1);
chenzhai 0:5ca44aa85a30 89 pushbutt.rise(&bushbutt_handler); //enable mbed trigger detect
Vigneshwar 1:6ec19e5615d9 90 accinterupt.rise(&acc_int_handler);
chenzhai 0:5ca44aa85a30 91 RegdumpAcc();
chenzhai 0:5ca44aa85a30 92
Vigneshwar 1:6ec19e5615d9 93
chenzhai 0:5ca44aa85a30 94 if(true)
Vigneshwar 1:6ec19e5615d9 95 for(int i=0; i<=100000; i++) {
Vigneshwar 1:6ec19e5615d9 96 wait(0.01);
Vigneshwar 1:6ec19e5615d9 97
Vigneshwar 1:6ec19e5615d9 98 if(ReadLed==1) ReadLed=0;
Vigneshwar 1:6ec19e5615d9 99 else ReadLed=1;
Vigneshwar 1:6ec19e5615d9 100
Vigneshwar 1:6ec19e5615d9 101 accel.getOutput(accel_read);
Vigneshwar 1:6ec19e5615d9 102
Vigneshwar 1:6ec19e5615d9 103 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],
Vigneshwar 1:6ec19e5615d9 104 gyro.getGyroX(), gyro.getGyroY(), gyro.getGyroZ(), (int16_t)gyro.getTemperature());
Vigneshwar 1:6ec19e5615d9 105
Vigneshwar 1:6ec19e5615d9 106 rn42.printf(str);
chenzhai 0:5ca44aa85a30 107
Vigneshwar 1:6ec19e5615d9 108 double d_accelx=(double) (int16_t)accel_read[0];
Vigneshwar 1:6ec19e5615d9 109 double d_accely=(double) (int16_t)accel_read[1];
Vigneshwar 1:6ec19e5615d9 110 double d_accelz=(double) (int16_t)accel_read[2];
Vigneshwar 1:6ec19e5615d9 111 double wp=6.28e-2; //IIR pole for DC cancellation
Vigneshwar 1:6ec19e5615d9 112 double w_x,w_y,w_z;
Vigneshwar 1:6ec19e5615d9 113 double dc_x,dc_y,dc_z;
Vigneshwar 1:6ec19e5615d9 114
Vigneshwar 1:6ec19e5615d9 115 //IIR calculate DC term in acceleration
Vigneshwar 1:6ec19e5615d9 116 w_x=d_accelx+ (1-wp)*w_last_x;
Vigneshwar 1:6ec19e5615d9 117 dc_x=wp*w_x;
Vigneshwar 1:6ec19e5615d9 118 w_last_x=w_x;
Vigneshwar 1:6ec19e5615d9 119 w_y=d_accely+ (1-wp)*w_last_y;
Vigneshwar 1:6ec19e5615d9 120 dc_y=wp*w_y;
Vigneshwar 1:6ec19e5615d9 121 w_last_y=w_y;
Vigneshwar 1:6ec19e5615d9 122 w_z=d_accelz+ (1-wp)*w_last_z;
Vigneshwar 1:6ec19e5615d9 123 dc_z=wp*w_z;
Vigneshwar 1:6ec19e5615d9 124 w_last_z=w_z;
Vigneshwar 1:6ec19e5615d9 125
Vigneshwar 1:6ec19e5615d9 126 d_accelx=d_accelx-dc_x;
Vigneshwar 1:6ec19e5615d9 127 d_accely=d_accely-dc_y;
Vigneshwar 1:6ec19e5615d9 128 d_accelz=d_accelz-dc_z;
Vigneshwar 1:6ec19e5615d9 129 double mag_accel=d_accelx*d_accelx+d_accely*d_accely+d_accelz*d_accelz;
Vigneshwar 1:6ec19e5615d9 130 mag_accel=sqrt(mag_accel);
Vigneshwar 1:6ec19e5615d9 131 //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);
Vigneshwar 1:6ec19e5615d9 132 RegPeekAcc(0x30);
Vigneshwar 1:6ec19e5615d9 133
Vigneshwar 1:6ec19e5615d9 134 if(autosleep_counter>0) autosleep_counter-=1;
Vigneshwar 1:6ec19e5615d9 135 else {
Vigneshwar 1:6ec19e5615d9 136 if(mag_accel<15) {
Vigneshwar 1:6ec19e5615d9 137 // sleep1();
Vigneshwar 1:6ec19e5615d9 138 autosleep_counter=30;
Vigneshwar 1:6ec19e5615d9 139 }
Vigneshwar 1:6ec19e5615d9 140 }
Vigneshwar 1:6ec19e5615d9 141
Vigneshwar 1:6ec19e5615d9 142 /*
Vigneshwar 1:6ec19e5615d9 143 pc.printf("Gyro[%5i, %5i, %5i] Accel[%4i, %4i, %4i]\n\t",
Vigneshwar 1:6ec19e5615d9 144 gyro.getGyroX(), gyro.getGyroY(), gyro.getGyroZ(),
Vigneshwar 1:6ec19e5615d9 145 (int16_t)accel_read[0], (int16_t)accel_read[1], (int16_t)accel_read[2]);
Vigneshwar 1:6ec19e5615d9 146 */
chenzhai 0:5ca44aa85a30 147 }
chenzhai 0:5ca44aa85a30 148
chenzhai 0:5ca44aa85a30 149
chenzhai 0:5ca44aa85a30 150 }
chenzhai 0:5ca44aa85a30 151
chenzhai 0:5ca44aa85a30 152
chenzhai 0:5ca44aa85a30 153 double twocomp16_to_double(int16_t x)
Vigneshwar 1:6ec19e5615d9 154 {
Vigneshwar 1:6ec19e5615d9 155 double a;
Vigneshwar 1:6ec19e5615d9 156 a= double(x);
Vigneshwar 1:6ec19e5615d9 157 return a;
chenzhai 0:5ca44aa85a30 158 }
chenzhai 0:5ca44aa85a30 159 int16_t double_to_twocomp16(double x)
Vigneshwar 1:6ec19e5615d9 160 {
Vigneshwar 1:6ec19e5615d9 161 int16_t a;
Vigneshwar 1:6ec19e5615d9 162 a=(signed int16_t) x;
Vigneshwar 1:6ec19e5615d9 163 return a;
chenzhai 0:5ca44aa85a30 164 }
chenzhai 0:5ca44aa85a30 165
chenzhai 0:5ca44aa85a30 166 char RegPeekAcc(char address)
chenzhai 0:5ca44aa85a30 167 {
chenzhai 0:5ca44aa85a30 168 char value=accel.SingleByteRead(address);
Vigneshwar 1:6ec19e5615d9 169 //pc.printf("AccReg address: %X value: %X\n\r",address,value);
chenzhai 0:5ca44aa85a30 170 return value;
chenzhai 0:5ca44aa85a30 171 }
chenzhai 0:5ca44aa85a30 172 void RegPokeAcc(char address, char value)
chenzhai 0:5ca44aa85a30 173 {
chenzhai 0:5ca44aa85a30 174 accel.SingleByteWrite(address,value);
chenzhai 0:5ca44aa85a30 175 }
chenzhai 0:5ca44aa85a30 176
chenzhai 0:5ca44aa85a30 177 void AccCalib()
Vigneshwar 1:6ec19e5615d9 178 {
Vigneshwar 1:6ec19e5615d9 179 CalLed=1;
Vigneshwar 1:6ec19e5615d9 180 double acc0=0,acc1=0,acc2=0;
Vigneshwar 1:6ec19e5615d9 181 double sum0=0,sum1=0,sum2=0;
chenzhai 0:5ca44aa85a30 182 wait(0.1) ;
chenzhai 0:5ca44aa85a30 183 accel.getOutput(accel_read);
Vigneshwar 1:6ec19e5615d9 184 // RegPokeAcc(0x1e,offsetcal((int16_t)accel_read[0]));
Vigneshwar 1:6ec19e5615d9 185 // RegPokeAcc(0x1f,offsetcal((int16_t)accel_read[1]));
chenzhai 0:5ca44aa85a30 186 // RegPokeAcc(0x20,offsetcal((int16_t)accel_read[2]));
Vigneshwar 1:6ec19e5615d9 187 for(int n=1; n<=10; n++) {
chenzhai 0:5ca44aa85a30 188 wait(0.1);
chenzhai 0:5ca44aa85a30 189 accel.getOutput(accel_read);
chenzhai 0:5ca44aa85a30 190 acc0=(double)(int16_t)accel_read[0];
chenzhai 0:5ca44aa85a30 191 acc1=(double)(int16_t)accel_read[1];
chenzhai 0:5ca44aa85a30 192 acc2=(double)(int16_t)accel_read[2];
Vigneshwar 1:6ec19e5615d9 193 sum0+=acc0;
Vigneshwar 1:6ec19e5615d9 194 sum1+=acc1;
Vigneshwar 1:6ec19e5615d9 195 sum2+=acc2;
chenzhai 0:5ca44aa85a30 196 }
chenzhai 0:5ca44aa85a30 197 sum0=sum0/10/(-4);
chenzhai 0:5ca44aa85a30 198 sum1=sum1/10/(-4);
Vigneshwar 1:6ec19e5615d9 199 sum2=sum2/10/(-4);
chenzhai 0:5ca44aa85a30 200 int16_t acc0_16=(signed int16_t)sum0;
chenzhai 0:5ca44aa85a30 201 int16_t acc1_16=(signed int16_t)sum1;
Vigneshwar 1:6ec19e5615d9 202 int16_t acc2_16=(signed int16_t)sum2;
Vigneshwar 1:6ec19e5615d9 203 RegPokeAcc(0x1e,acc0_16);
Vigneshwar 1:6ec19e5615d9 204 RegPokeAcc(0x1f,acc1_16);
Vigneshwar 1:6ec19e5615d9 205 RegPokeAcc(0x20,acc2_16);
Vigneshwar 1:6ec19e5615d9 206 // pc.printf("Calibration. Calculated offset %i %i %i\n\r",acc0_16,acc1_16,acc2_16);
chenzhai 0:5ca44aa85a30 207 int valid=1;
Vigneshwar 1:6ec19e5615d9 208 for(int n=1; n<=10; n++) {
Vigneshwar 1:6ec19e5615d9 209 wait(0.1);
Vigneshwar 1:6ec19e5615d9 210 accel.getOutput(accel_read);
Vigneshwar 1:6ec19e5615d9 211 accel.getOutput(accel_read);
Vigneshwar 1:6ec19e5615d9 212 acc0=(double)(int16_t)accel_read[0];
Vigneshwar 1:6ec19e5615d9 213 if(abs(acc0)>50) valid=0;
Vigneshwar 1:6ec19e5615d9 214 acc1=(double)(int16_t)accel_read[1];
Vigneshwar 1:6ec19e5615d9 215 if(abs(acc1)>50) valid=0;
Vigneshwar 1:6ec19e5615d9 216 acc2=(double)(int16_t)accel_read[2];
Vigneshwar 1:6ec19e5615d9 217 if(abs(acc2)>50) valid=0;
Vigneshwar 1:6ec19e5615d9 218 // pc.printf("verifying cal %i,%i,%i \n\r", (int16_t)accel_read[0], (int16_t)accel_read[1], (int16_t)accel_read[2]);
Vigneshwar 1:6ec19e5615d9 219 // pc.printf("verifying cal (double)%f %f %f\n\r",acc0,acc1,acc2);
Vigneshwar 1:6ec19e5615d9 220 // pc.printf("valid= %d \n\r",valid);
Vigneshwar 1:6ec19e5615d9 221 }
chenzhai 0:5ca44aa85a30 222 // if(valid==1)
Vigneshwar 1:6ec19e5615d9 223 CalLed=0;
chenzhai 0:5ca44aa85a30 224 // else AccCalib();
chenzhai 0:5ca44aa85a30 225 }
chenzhai 0:5ca44aa85a30 226
chenzhai 0:5ca44aa85a30 227 void RegdumpAcc()
chenzhai 0:5ca44aa85a30 228 {
Vigneshwar 1:6ec19e5615d9 229 //pc.printf("\r---------a new regdump start\n\r-------");
Vigneshwar 1:6ec19e5615d9 230 //pc.printf("Acc DeviceID \t");
Vigneshwar 1:6ec19e5615d9 231 RegPeekAcc(0x00);
Vigneshwar 1:6ec19e5615d9 232 //pc.printf("X_asis: \t");
Vigneshwar 1:6ec19e5615d9 233 RegPeekAcc(0x1e);
Vigneshwar 1:6ec19e5615d9 234 //pc.printf("Y_asis: \t");
Vigneshwar 1:6ec19e5615d9 235 RegPeekAcc(0x1f);
Vigneshwar 1:6ec19e5615d9 236 //pc.printf("Z_asis: \t");
Vigneshwar 1:6ec19e5615d9 237 RegPeekAcc(0x20);
Vigneshwar 1:6ec19e5615d9 238 //pc.printf("Act Thresh: \t");
Vigneshwar 1:6ec19e5615d9 239 RegPeekAcc(0x24);
Vigneshwar 1:6ec19e5615d9 240 //pc.printf("Axis Enable: \t");
Vigneshwar 1:6ec19e5615d9 241 RegPeekAcc(0x27);
Vigneshwar 1:6ec19e5615d9 242 //pc.printf("Interpt Ctrl: \t");
Vigneshwar 1:6ec19e5615d9 243 RegPeekAcc(0x2e);
Vigneshwar 1:6ec19e5615d9 244 //pc.printf("Interpt Mapping: \t");
Vigneshwar 1:6ec19e5615d9 245 RegPeekAcc(0x2f);
chenzhai 0:5ca44aa85a30 246 //pc.printf("Source of interupt:\t");RegPeekAcc(0x30);
Vigneshwar 1:6ec19e5615d9 247 //pc.printf("Data format control\t");
Vigneshwar 1:6ec19e5615d9 248 RegPeekAcc(0x31);
Vigneshwar 1:6ec19e5615d9 249 //pc.printf("FIFO Ctrl:\t");
Vigneshwar 1:6ec19e5615d9 250 RegPeekAcc(0x38);
Vigneshwar 1:6ec19e5615d9 251 //pc.printf("FIFO Status:\t");
Vigneshwar 1:6ec19e5615d9 252 RegPeekAcc(0x39);
chenzhai 0:5ca44aa85a30 253 }
chenzhai 0:5ca44aa85a30 254
chenzhai 0:5ca44aa85a30 255 void sleep1()
chenzhai 0:5ca44aa85a30 256 {
Vigneshwar 1:6ec19e5615d9 257 WakeLed=0;
Vigneshwar 1:6ec19e5615d9 258 SleepLed=1;
Vigneshwar 1:6ec19e5615d9 259 sleep();
chenzhai 0:5ca44aa85a30 260
chenzhai 0:5ca44aa85a30 261 }