Mini Project 10: Displaying stuff from day 7
Dependencies: DmTouch_UniGraphic UniGraphic mbed
compass_sensor.cpp@12:bd1d030d5c30, 2017-01-17 (annotated)
- Committer:
- swescott17
- Date:
- Tue Jan 17 20:55:08 2017 +0000
- Revision:
- 12:bd1d030d5c30
- Parent:
- 11:482a9abbc448
Ticker completed
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
swescott17 | 0:1ebe73e062a7 | 1 | |
swescott17 | 0:1ebe73e062a7 | 2 | #include "compass_sensor.h" |
swescott17 | 0:1ebe73e062a7 | 3 | |
swescott17 | 0:1ebe73e062a7 | 4 | char addr = 0x1E<<1; //address of the compass |
swescott17 | 0:1ebe73e062a7 | 5 | char rawBytes[6]; //raw data extracted from the read of the compass sensor |
swescott17 | 0:1ebe73e062a7 | 6 | short data[6]; //rawBytes is the entered into data |
swescott17 | 0:1ebe73e062a7 | 7 | short magComponent[3]; //x, z, y |
swescott17 | 0:1ebe73e062a7 | 8 | double heading; //the "degree" variable |
swescott17 | 0:1ebe73e062a7 | 9 | double sum; //used for calculating the average |
swescott17 | 0:1ebe73e062a7 | 10 | |
swescott17 | 11:482a9abbc448 | 11 | void compass_config(void) //configures the sensor |
swescott17 | 0:1ebe73e062a7 | 12 | { |
swescott17 | 0:1ebe73e062a7 | 13 | i2c_port.start(); //starts the compass |
swescott17 | 0:1ebe73e062a7 | 14 | i2c_port.write(addr); //sets the address |
swescott17 | 0:1ebe73e062a7 | 15 | i2c_port.write(0x02); //writes the bytes |
swescott17 | 0:1ebe73e062a7 | 16 | i2c_port.write(0x00); //writes the bytes |
swescott17 | 0:1ebe73e062a7 | 17 | i2c_port.stop(); //stops |
swescott17 | 0:1ebe73e062a7 | 18 | } |
swescott17 | 0:1ebe73e062a7 | 19 | |
swescott17 | 0:1ebe73e062a7 | 20 | double calculation(void) |
swescott17 | 0:1ebe73e062a7 | 21 | { |
swescott17 | 0:1ebe73e062a7 | 22 | i2c_port.read(addr,rawBytes,6); //reads bytes into rawBytes, there are 6 array |
swescott17 | 0:1ebe73e062a7 | 23 | for (int i = 0; i<6; i++) //read rawBytes array into data array |
swescott17 | 0:1ebe73e062a7 | 24 | { |
swescott17 | 0:1ebe73e062a7 | 25 | data[i] = rawBytes[i]; |
swescott17 | 0:1ebe73e062a7 | 26 | } |
swescott17 | 0:1ebe73e062a7 | 27 | magComponent[0] = data[0]<<8 | data[1]; //x-component |
swescott17 | 0:1ebe73e062a7 | 28 | magComponent[2] = data[2]<<8 | data[3]; //z-component |
swescott17 | 0:1ebe73e062a7 | 29 | magComponent[1] = data[4]<<8 | data[5]; //y-component |
swescott17 | 0:1ebe73e062a7 | 30 | |
swescott17 | 0:1ebe73e062a7 | 31 | heading = atan2((double)magComponent[1], (double)magComponent[0]); //equation for getting the degrees in radians |
swescott17 | 0:1ebe73e062a7 | 32 | heading = heading*(180.0/3.1416); //radians to degrees |
swescott17 | 0:1ebe73e062a7 | 33 | heading = (heading + 14.85); //adjust for Spokane |
swescott17 | 0:1ebe73e062a7 | 34 | heading = heading-(0.13); //declination in years |
swescott17 | 0:1ebe73e062a7 | 35 | return heading; |
swescott17 | 0:1ebe73e062a7 | 36 | } |
swescott17 | 0:1ebe73e062a7 | 37 | |
swescott17 | 11:482a9abbc448 | 38 | float compass_n() |
swescott17 | 0:1ebe73e062a7 | 39 | { |
swescott17 | 0:1ebe73e062a7 | 40 | wait(0.02); |
swescott17 | 0:1ebe73e062a7 | 41 | i2c_port.start(); //starts the compass |
swescott17 | 0:1ebe73e062a7 | 42 | i2c_port.write(addr); //compass is given its address again |
swescott17 | 0:1ebe73e062a7 | 43 | i2c_port.write(0x03); //told to given 3 bytes |
swescott17 | 0:1ebe73e062a7 | 44 | i2c_port.stop(); |
swescott17 | 12:bd1d030d5c30 | 45 | for (int m = 0; m<5; m++) //average of 5 |
swescott17 | 0:1ebe73e062a7 | 46 | { |
swescott17 | 0:1ebe73e062a7 | 47 | heading = calculation(); //function that calculates the degrees from N |
swescott17 | 0:1ebe73e062a7 | 48 | sum += heading; //sum of the heading(s) |
swescott17 | 0:1ebe73e062a7 | 49 | } |
swescott17 | 12:bd1d030d5c30 | 50 | heading = sum/5; //the average of the heading(s) collected |
swescott17 | 0:1ebe73e062a7 | 51 | if (heading > 180){ |
swescott17 | 0:1ebe73e062a7 | 52 | heading = heading - 360; //if greater than 180, then subtract 360 since it was too large |
swescott17 | 0:1ebe73e062a7 | 53 | } |
swescott17 | 0:1ebe73e062a7 | 54 | if (heading < -180){ |
swescott17 | 0:1ebe73e062a7 | 55 | heading = heading +360; //if less than -180, then add 360 since it was too small |
swescott17 | 0:1ebe73e062a7 | 56 | } |
swescott17 | 11:482a9abbc448 | 57 | return heading; |
swescott17 | 0:1ebe73e062a7 | 58 | } |