Using LSM303DLM Kompass-Module and Library for m3pi-Robot.
Dependencies: LSM303DLM m3pi mbed
main.cpp
- Committer:
- rahm
- Date:
- 2014-11-25
- Revision:
- 0:ccf3df1312aa
File content as of revision 0:ccf3df1312aa:
#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); }