Diff: compass_sensor.cpp

Revision:

0:1ebe73e062a7

Child:

11:482a9abbc448

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/compass_sensor.cpp	Tue Jan 17 18:14:15 2017 +0000
@@ -0,0 +1,67 @@
+
+#include "compass_sensor.h"
+
+char addr = 0x1E<<1; //address of the compass
+char rawBytes[6]; //raw data extracted from the read of the compass sensor
+short data[6]; //rawBytes is the entered into data
+short magComponent[3]; //x, z, y
+double heading; //the "degree" variable
+double sum; //used for calculating the average
+
+int compass_config(void) //returns the number of samples and configures the sensor
+{  
+        pc.printf("Enter a number of samples you would like.\r\n");
+        int c;
+        pc.scanf("%c", &c);
+        pc.printf("\r\nValue:%c", c);
+        if (c != 1 && c != 2 && c != 4 && c != 8) //if the average number is not 1, 2, 4, and 8, then set it to 1
+        {
+            c = 1;
+        }
+    i2c_port.start(); //starts the compass
+    i2c_port.write(addr); //sets the address
+    i2c_port.write(0x02); //writes the bytes
+    i2c_port.write(0x00); //writes the bytes
+    i2c_port.stop(); //stops
+        return (c); //returns the number of samples
+}
+
+double calculation(void)
+{
+                i2c_port.read(addr,rawBytes,6); //reads bytes into rawBytes, there are 6 array 
+                for (int i = 0; i<6; i++) //read rawBytes array into data array
+                {
+                    data[i] = rawBytes[i];
+                }
+                magComponent[0] = data[0]<<8 | data[1]; //x-component
+                magComponent[2] = data[2]<<8 | data[3]; //z-component
+                magComponent[1] = data[4]<<8 | data[5]; //y-component
+        
+            heading = atan2((double)magComponent[1], (double)magComponent[0]); //equation for getting the degrees in radians
+            heading = heading*(180.0/3.1416); //radians to degrees
+            heading = (heading + 14.85); //adjust for Spokane
+            heading = heading-(0.13); //declination in years
+            return heading;
+}
+
+void compass_n(int c)
+{        
+                wait(0.02);
+                i2c_port.start(); //starts the compass
+                i2c_port.write(addr); //compass is given its address again
+                i2c_port.write(0x03); //told to given 3 bytes
+                i2c_port.stop();
+            for (int m = 0; m<c; m++) 
+            {
+                heading = calculation(); //function that calculates the degrees from N
+                sum += heading; //sum of the heading(s)
+            }
+            heading = sum/c; //the average of the heading(s) collected
+            if (heading > 180){
+                heading = heading - 360; //if greater than 180, then subtract 360 since it was too large
+                }
+            if (heading < -180){
+                heading = heading +360;  //if less than -180, then add 360 since it was too small
+                }
+            pc.printf("North is %.2f. \n\r ", heading); //print the averaged reading
+}
\ No newline at end of file