Diff: main.cpp

Revision:

0:ccf3df1312aa

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue Nov 25 13:52:55 2014 +0000
@@ -0,0 +1,63 @@
+#include "mbed.h"
+#include <math.h>
+#include "LSM303DLM.h"
+#include "m3pi.h"
+
+#define YOFF  100.0    // Y-Offset
+#define XOFF  2.0      // X-Offset
+#define WOFF  12.0     // Winkel-Offset
+
+//Konstruktoren
+//Serial pc(USBTX,USBRX);        // Für Testausgabe auf PC
+LSM303DLM compass(p28, p27);
+m3pi robo;
+
+// Funktionsprototyp calcHeading
+double calcHeading(int *mag);
+
+// Funktion main
+int main()
+{
+  double hdg;
+  int mag[3];
+
+  //pc.printf("LSM303DLH Test    \n\r");
+  robo.locate(0,0);
+  robo.print("LSM303DL",8);
+  while(1)
+  {
+    compass.readMag(mag);
+    hdg = calcHeading(mag);
+    
+    //pc.printf(" %.1f   ", hdg);
+    robo.locate(0,1);
+    robo.printf(" %.1f\xdf   ", hdg);       // \xdf = °-Symbol im Display-Zeichensatz
+    wait(0.4);
+  }
+}
+
+double calcHeading(int *mag)
+{
+    double x,y;
+    double hdg;
+    
+    x = mag[0] + XOFF;                  // X-Achsen Ausgleich
+    y = mag[1] + YOFF;                  // Y-Achsen Ausgleich
+    
+    hdg = atan(y/x);
+    hdg *= 57.2958;                     // Umrechnung von Bogen- nach Winkelmass
+    if (x > 0)
+    {
+        if(y>0) hdg = hdg;              // Korrektur 1. Quadrant
+        else    hdg = 360.0 + hdg;      // Korrektur 4. Quadrant
+    }
+    else
+    {   
+        if(y>0) hdg = 180.0 + hdg;      // Korrektur 2. Quadrant
+        else hdg = 180.0 + hdg;         // Korrektur 3. Quadrant
+    }
+    
+    hdg -= WOFF;                        // Korrektur Winkel (Vorsicht: Bei Winkeln < WOFF wird hdg negativ!)
+    if (hdg <0) hdg = 360 + hdg;        // Winkel soll nicht negativ werden! (Bsp.: -5° => 355°)
+    return (hdg);
+}
\ No newline at end of file