Mission Accomplished

Dependencies:   TextLCD mbed

Committer:
pstephens18
Date:
Wed Jan 20 01:01:04 2016 +0000
Revision:
0:96cf16c88314
Final Mission Accomplished
;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
pstephens18 0:96cf16c88314 1 #include "Compass.h"
pstephens18 0:96cf16c88314 2 extern I2C acc;
pstephens18 0:96cf16c88314 3 char rawBytes[6];
pstephens18 0:96cf16c88314 4 short data[6];
pstephens18 0:96cf16c88314 5 short magComponents[3];
pstephens18 0:96cf16c88314 6 double heading;
pstephens18 0:96cf16c88314 7 const int addr1 = (0x1E << 1);
pstephens18 0:96cf16c88314 8 char config[2];
pstephens18 0:96cf16c88314 9 char init[2];
pstephens18 0:96cf16c88314 10 double Compass()
pstephens18 0:96cf16c88314 11 {
pstephens18 0:96cf16c88314 12 config[0]= 0x02;
pstephens18 0:96cf16c88314 13 config[1] = 0x00;
pstephens18 0:96cf16c88314 14 acc.write(addr1, config,2);
pstephens18 0:96cf16c88314 15 init[0]=0x03;
pstephens18 0:96cf16c88314 16 while(1)
pstephens18 0:96cf16c88314 17 {
pstephens18 0:96cf16c88314 18 acc.write(addr1,init,1);
pstephens18 0:96cf16c88314 19 acc.read(addr1,rawBytes,6);
pstephens18 0:96cf16c88314 20
pstephens18 0:96cf16c88314 21
pstephens18 0:96cf16c88314 22 for (int i = 0; i<6; i++)
pstephens18 0:96cf16c88314 23 {
pstephens18 0:96cf16c88314 24 data[i] = rawBytes[i];
pstephens18 0:96cf16c88314 25 }
pstephens18 0:96cf16c88314 26
pstephens18 0:96cf16c88314 27 magComponents[0] = data[0]<<8 | data[1];
pstephens18 0:96cf16c88314 28 magComponents[1] = data[4]<<8 | data[5];
pstephens18 0:96cf16c88314 29 magComponents[2] = data[2]<<8 | data[3];
pstephens18 0:96cf16c88314 30
pstephens18 0:96cf16c88314 31 heading = atan2((double)magComponents[1],(double)magComponents[0]); // Angle in Radians
pstephens18 0:96cf16c88314 32 heading = (heading*180)/3.14159; // Convert to Degrees
pstephens18 0:96cf16c88314 33 heading = heading + 14.85; // Find True North
pstephens18 0:96cf16c88314 34 if(heading > 180)
pstephens18 0:96cf16c88314 35 {
pstephens18 0:96cf16c88314 36 heading -= 360;
pstephens18 0:96cf16c88314 37 }
pstephens18 0:96cf16c88314 38
pstephens18 0:96cf16c88314 39 return heading;
pstephens18 0:96cf16c88314 40 }
pstephens18 0:96cf16c88314 41 }
pstephens18 0:96cf16c88314 42
pstephens18 0:96cf16c88314 43
pstephens18 0:96cf16c88314 44