Grove 3 axlig Digital kompass (4 / 4 steg)
Steg 4: kod
Koden är ganska enkel och allt den gör är läser kompass avläsningarna och skriver ut det på en seriell monitor. Kompassen kalibrerar ett par sekunder när du kör kod gör visst inte att rotera eller störa kompassen medan detta händer och när den är klar bör du få en fin ström av data från sensorn.
< p > / / referens I2C biblioteket < br > #include < wire.h >
Referens HMC5883L kompass biblioteket
#include < hmc5883l.h >< /hmc5883l.h >< /wire.h >< /p >< p > / / Store vår kompass som en variabel.
HMC5883L kompass;
Spela in eventuella fel som kan uppstå i kompassen.
int fel = 0; < /p >< p > / / ut setup rutin, här kommer vi konfigurera microcontroller och kompass.
void setup()
{
Initiera serieporten.
Serial.BEGIN(9600); < /p >< p > Serial.println ("Start I2C gränssnittet.");
Wire.BEGIN(); Starta den I2C gränssnitt. < /p >< p > Serial.println ("konstruera nya HMC5883L");
Serial.println ("Ange skala till +/-1.3 Ga");
fel = compass.setScale(1.3); Ange omfattningen av kompassen.
om (fel! = 0) / / om det finns ett fel, skriva ut den.
Serial.println(Compass.getErrorText(Error));
Serial.println ("inställningen mätning läge till kontinuerlig.");
fel = compass.setMeasurementMode(MEASUREMENT_CONTINUOUS); Inställd kontinuerlig mätning läge
om (fel! = 0) / / om det finns ett fel, skriva ut den.
Serial.println(Compass.getErrorText(Error));
} < /p >< p > / / vår huvudsakliga program loop.
void loop()
{
Retrive raw värdena från kompassen (inte skalas).
MagnetometerRaw rå = compass.readRawAxis();
Hämta skalad värdena från kompassen (skalas till den konfigurerade skalan).
MagnetometerScaled skalas = compass.readScaledAxis();
Värdena används som så:
int MilliGauss_OnThe_XAxis = skalas. XAxis; / / (eller YAxis eller ZAxis) < /p >< p > / / Beräkna rubrik när magnetometer är nivå, sedan korrekt för tecken på axeln.
float rubrik = ARCTAN2 (skalas. YAxis, skalas. XAxis);
När du har din riktning, måste du sedan lägga till din "deklination vinkel", som är "Fel" av det magnetiskt sätter in i din plats.
Hitta din här: http://www.magnetic-declination.com/
Min är:-2 37' som är-2.617 grader eller (som vi behöver)-0.0456752665 radianer, jag kommer att använda-0.0457
Om du inte hittar din deklination, kommenterar ut dessa två linjer, blir din kompass lite.
flyta declinationAngle =-0.0457;
rubriken += declinationAngle;
Korrigera för när tecken är omvända.
IF(heading < 0)
rubrik + = 2 * PI;
Kontrollera om sjal på grund av tillägg av deklination.
om (rubrik > 2 * PI)
rubrik-= 2 * PI;
Konvertera radianer till grader för läsbarhet.
flyta headingDegrees = rubrik * 180/M_PI; < /P >< p > / / Output data via den seriella porten.
Utgång (rå, skalad, rubrik, headingDegrees); < /p >< p > / / normalt vi skulle fördröja tillämpningen av 66ms så att slingan
att köra på 15Hz (standard bandbredd för HMC5883L).
Men eftersom vi har en lång följetong ut (104ms på 9600) vi kommer att låta
Det arbetar med dess naturliga hastighet.
fördröjning (66); //of kurs det kan fördröjas längre.
} < /p >< p > / / Output data ner den seriella porten.
void utgång (MagnetometerRaw raw, MagnetometerScaled skalas, rubrik, float float headingDegrees)
{
Serial.Print("RAW:\t");
Serial.Print (rå. XAxis);
Serial.Print("");
Serial.Print (rå. YAxis);
Serial.Print("");
Serial.Print (rå. ZAxis);
Serial.Print ("\tScaled:\t");
Serial.Print (skalas. XAxis);
Serial.Print("");
Serial.Print (skalas. YAxis);
Serial.Print("");
Serial.Print (skalas. ZAxis); < /p >< p > Serial.print ("\tHeading:\t");
Serial.Print(Heading);
Serial.Print ("radianer \t");
Serial.Print(headingDegrees);
Serial.println ("grader \t");
} < /p >