Chen Zhai / Mbed 2 deprecated 6dof_new_workwith_v2

project

main.cpp@0:5ca44aa85a30, 2013-03-12 (annotated)

Committer:
chenzhai
Date:
Tue Mar 12 21:56:17 2013 +0000
Revision:
0:5ca44aa85a30
6dof sensor wireless

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