main.cpp

00001 #include "mbed.h"
00002 #include <math.h>
00003 #include "LSM303DLM.h"
00004 #include "m3pi.h"
00005 
00006 #define YOFF  100.0    // Y-Offset
00007 #define XOFF  2.0      // X-Offset
00008 #define WOFF  12.0     // Winkel-Offset
00009 
00010 //Konstruktoren
00011 //Serial pc(USBTX,USBRX);        // Für Testausgabe auf PC
00012 LSM303DLM compass(p28, p27);
00013 m3pi robo;
00014 
00015 // Funktionsprototyp calcHeading
00016 double calcHeading(int *mag);
00017 
00018 // Funktion main
00019 int main()
00020 {
00021   double hdg;
00022   int mag[3];
00023 
00024   //pc.printf("LSM303DLH Test    \n\r");
00025   robo.locate(0,0);
00026   robo.print("LSM303DL",8);
00027   while(1)
00028   {
00029     compass.readMag(mag);
00030     hdg = calcHeading(mag);
00031     
00032     //pc.printf(" %.1f   ", hdg);
00033     robo.locate(0,1);
00034     robo.printf(" %.1f\xdf   ", hdg);       // \xdf = °-Symbol im Display-Zeichensatz
00035     wait(0.4);
00036   }
00037 }
00038 
00039 double calcHeading(int *mag)
00040 {
00041     double x,y;
00042     double hdg;
00043     
00044     x = mag[0] + XOFF;                  // X-Achsen Ausgleich
00045     y = mag[1] + YOFF;                  // Y-Achsen Ausgleich
00046     
00047     hdg = atan(y/x);
00048     hdg *= 57.2958;                     // Umrechnung von Bogen- nach Winkelmass
00049     if (x > 0)
00050     {
00051         if(y>0) hdg = hdg;              // Korrektur 1. Quadrant
00052         else    hdg = 360.0 + hdg;      // Korrektur 4. Quadrant
00053     }
00054     else
00055     {   
00056         if(y>0) hdg = 180.0 + hdg;      // Korrektur 2. Quadrant
00057         else hdg = 180.0 + hdg;         // Korrektur 3. Quadrant
00058     }
00059     
00060     hdg -= WOFF;                        // Korrektur Winkel (Vorsicht: Bei Winkeln < WOFF wird hdg negativ!)
00061     if (hdg <0) hdg = 360 + hdg;        // Winkel soll nicht negativ werden! (Bsp.: -5° => 355°)
00062     return (hdg);
00063 }