Kompass-Programmierung
Hier einige Erläuterungen zur Ermittlung der Himmelsrichtungen
Die Messwerte X_VALUE un Y_VALUE müssen nun noch in Himmelsrichtungen bzw. Gradzahlen umgewandelt werden.
Die Messwerte meines HDMM01-Moduls liegen etwa in diesem Bereich:
XMAX=2176 - XMIN=1977 - YMAX=2122 - YMIN=1902
Das ergibt etwa das folgende Bild, wenn man alle möglichen Messwerte in ein Diagramm eintragen würde:
Jetzt muss nur noch der Nullpunkt des Kreises ermittelt werden:
Der Nullpunkt berechnet sich wie folgt:
X_NULLPUNKT = (XMAX-XMIN)/2 + XMIN
Y_NULLPUNKT = (YMAX-YMIN)/2 + YMIN
Schließlich wird der Mittelpunkt des Messwert-Kreises rechnerisch so verschoben, dass er im Koordinatenursprung liegt. Soviel zur Theorie...
Nun habe ich versucht, die auf den Nullpunkt normierten (und gerundeten) Messwerte einmal in eine "Windrose" einzutragen:
Dabei musste ich verblüfft feststellen, dass die X- und die Y-Achse ja eigentlich "vertauscht" sind. Die Angaben wie 100, 0 bzw. 0, 100 usw. sind die jeweiligen (gerundeten) X/Y-Wertepaare.
Eigentlich ergibt sich kein ganz idealer Kreis sondern eher eine Ellipse.
Die Winkel in den 4 Quadranten werden nun über den Arcus-Tangens ermittelt. Das ist die Umkehrfunktion des Tangens.
Es gilt im rechtwinkligen Dreieck nämlich:
tan (a) = Gegenkathete / Ankathete -> a = atan(Gegenkathete / Ankathete)
Die atan-Funktion gehört zum Standard-ANSI-C und gilt im Bereich von -Pi/2 bis + Pi/2.
Ich habe nun für jeden Quadranten eine eigene Rechnung aufgemacht und auch die Fälle Y/X=0 und X/Y=0 abgefangen (Division durch NULL).
Heraus kommt schließlich ein Winkel von 0 bis 360 Grad. Das war ja das auch Ziel.
In dieser Programmversion verwende ich während einer Kalibrierung CompassCalibrate(void) gemessene Werte für X_MAX, X_MIN, Y_MAX und Y_MIN. Dazu muss sich der PRO-BOT mehrfach drehen, damit er den vollen Wertebereich der X- und Y-Messwerte scannen kann.
Programmlisting:
/*******************************************************************************
Projektname: KOMPASS_NIKO_CC.cprj
Benötigte Libs´s: IntFunc_lib.cc
Routinen: kompass.cc, PRO-BOT128C_Lib_V2.cc
Autor: Niko
Datum: 3. 12. 2012
Funktion: Kompass starten und Winkel ausgeben
*******************************************************************************/
// Der MMC2120-Kompass-Chip besitzt ein Schreibregister, in das die Befehle ge-
// schrieben werden können sowie 5 Leseregister, in das der Inhalt des Status-
// registers und die Messwerte abgelegt werden:
// STATUS-Register - Adresse 0x00
// MSB des X-Messwertes - Adresse 0x01
// LSB des X-Messwertes - Adresse 0x02
// MSB des Y-Messwertes - Adresse 0x03
// LSB des Y-Messwertes - Adresse 0x04
// I2C-Adresse und Wartezeit definieren:
#define I2C_ADDR_KOMPASS 0x60 // fest eingestellt!
#define WAIT1 5 // Millisekunden
#define PI 3.1415
// Maximal/Minimalwerte für X und Y - durch Test ermittelt
#define X_MAX 1
#define X_MIN 5000
#define Y_MAX 1
#define Y_MIN 5000
// Register und Befehle des Kompass-Chips
#define WRITE_ADDRESS 0x00 // Adresse des Schreibregisters
#define READ_ADDRESS 0x00 // Basisadresse der 5 Leseregister
#define TAKE_MEASURE 0x01 // Befehl zum Start der Messung
#define SET_COIL 0x02 // Setzen der Magnetspule
#define RESET_COIL 0x04 // Löschen der Magnetspule
#define 12BIT_MASK 0x0FFF //Maskierung für 12-Bit-Werte
int XMAX, XMIN, YMAX, YMIN;
void main(void)
{
PRO_BOT128_INIT(); //PRO-BOT128 Setup
DELAY_MS(WAIT1);
XMAX = X_MAX;
XMIN = X_MIN;
YMAX = Y_MAX;
YMIN = Y_MIN;
CompassResetSetCoils();
CompassCalibrate();
while(1)
{
int Winkel;
Winkel=CompassGetDegree();
Msg_WriteText("Winkel:");
Msg_WriteInt(Winkel);
Msg_WriteChar(13);
}
}
void CompassResetSetCoils (void)
{
// RESET / SET Coils
I2C_Start();
I2C_Write(I2C_ADDR_KOMPASS);
I2C_Write(WRITE_ADDRESS); // als WAKE-UP
I2C_Write(RESET_COIL);
I2C_Stop();
DELAY_MS(WAIT1);
I2C_Start();
I2C_Write(I2C_ADDR_KOMPASS);
I2C_Write(WRITE_ADDRESS); // als WAKE-UP
I2C_Write(SET_COIL);
I2C_Stop();
DELAY_MS(WAIT1);
}
int CompassGetDegree(void)
{
byte MSB_X, LSB_X, MSB_Y, LSB_Y; //Rohdaten / Messwerte 8 Bit
int X_VALUE, Y_VALUE; // 12-Bit-Messwerte
int X_OFFSET, Y_OFFSET; // Nullpunkt des Messkreises
float X_NORMIERT, Y_NORMIERT; // Auf Nullpunkt normierte Messwerte
int mygrad; // fertig berechneter Winkel 0...360 Grad
// Messung starten und Lesen der 5 Byte des Kompassmoduls
// Die Reihenfolge der Befehle wird im Datenblatt so verlangt:
I2C_Start();
I2C_Write(I2C_ADDR_KOMPASS);
I2C_Write(WRITE_ADDRESS); // Basisadresse der Kompass-Register
I2C_Write(TAKE_MEASURE); // Messung starten
I2C_Stop();
DELAY_MS(WAIT1); // WAIT erforderlich für die Messung!
I2C_Start();
I2C_Write(I2C_ADDR_KOMPASS);
// STATUS-REGISTER auf Basisadresse soll nicht gelesen werden:
I2C_Write(READ_ADDRESS+1); // Setze Leseadresse+1 der Kompass-Lese-Register
I2C_Stop();
I2C_Start();
I2C_Write(I2C_ADDR_KOMPASS+1); // Lese-Adresse = I2C-Adresse + 1!
// Ersten Wert auslesn
MSB_X = I2C_Read_ACK();
// Zweiten Wert auslesen
LSB_X =I2C_Read_ACK();
// Dritten Wert auslesen
MSB_Y = I2C_Read_ACK();
// Vierten Wert auslesen
LSB_Y =I2C_Read_NACK(); //Hier KEIN ACKNOWLEDGE!
I2C_Stop();
DELAY_MS(WAIT1);
// Messwerte zu 12-Bit-Zahl zusammensetzen
X_VALUE = MSB_X*256 + LSB_X;
X_VALUE = X_VALUE & 12BIT_MASK;
Y_VALUE = MSB_Y*256 + LSB_Y;
Y_VALUE = Y_VALUE & 12BIT_MASK;
// Nullpunkt des Meswswertkreises ermittel:
X_OFFSET = (XMAX-XMIN)/2+XMIN;
Y_OFFSET = (YMAX-YMIN)/2+YMIN;
X_NORMIERT = X_VALUE-X_OFFSET;
Y_NORMIERT = Y_VALUE-Y_OFFSET;
Msg_WriteText("X_NORMIERT:");
Msg_WriteInt(X_NORMIERT);
Msg_WriteText("-Y_NORMIERT:");
Msg_WriteInt(Y_NORMIERT);
Msg_WriteChar(13);
// Fallunterscheidung nach Quadranten:
// NORD_OST
if ((X_NORMIERT<0)&&(Y_NORMIERT<0))
{
mygrad = atan(Y_NORMIERT/X_NORMIERT) * 180 / PI;
}
// SÜD_OST
if ((X_NORMIERT>0)&&(Y_NORMIERT<0))
{
mygrad = 90+atan(-X_NORMIERT/Y_NORMIERT) * 180 / PI;
}
// SÜD_WEST
if ((X_NORMIERT>0)&&(Y_NORMIERT>0))
{
mygrad = 180+atan(Y_NORMIERT/X_NORMIERT) * 180 / PI;
}
// NORD_WEST
if ((X_NORMIERT<0)&&(Y_NORMIERT>0))
{
mygrad = 270+atan(-X_NORMIERT/Y_NORMIERT) * 180 / PI;
}
if ((X_NORMIERT==0)&&(Y_NORMIERT<0))
{
mygrad = 90;
}
if ((Y_NORMIERT==0)&&(X_NORMIERT>0))
{
mygrad = 180;
}
if ((X_NORMIERT==0)&&(Y_NORMIERT>0))
{
mygrad = 270;
}
if ((Y_NORMIERT==0)&&(X_NORMIERT<0))
{
mygrad = 0;
}
return mygrad;
}
void CompassCalibrate(void)
{
byte MSB_X, LSB_X, MSB_Y, LSB_Y; //Rohdaten / Messwerte 8 Bit
int X_VALUE, Y_VALUE; // 12-Bit-Messwerte
DRIVE_ON();//ENABLE for Motore
MOTOR_DIR(1,0); //Richtung "Vorwärts" und "Rückwärts"
MOTOR_POWER(200,200);
int i;
for (i=0; i<1000; i++)
{
// PRO-BOT128 dreht ein kleines Stück und nimmt Messwerte
// Messung starten und Lesen der 5 Byte des Kompassmoduls
// Die Reihenfolge der Befehle wird im Datenblatt so verlangt:
I2C_Start();
I2C_Write(I2C_ADDR_KOMPASS);
I2C_Write(WRITE_ADDRESS); // Basisadresse der Kompass-Register
I2C_Write(TAKE_MEASURE); // Messung starten
I2C_Stop();
DELAY_MS(WAIT1); // WAIT erforderlich für die Messung!
I2C_Start();
I2C_Write(I2C_ADDR_KOMPASS);
// STATUS-REGISTER auf Basisadresse soll nicht gelesen werden:
I2C_Write(READ_ADDRESS+1); // Setze Leseadresse+1 der Kompass-Lese-Register
I2C_Stop();
I2C_Start();
I2C_Write(I2C_ADDR_KOMPASS+1); // Lese-Adresse = I2C-Adresse + 1!
// Ersten Wert auslesn
MSB_X = I2C_Read_ACK();
// Zweiten Wert auslesen
LSB_X =I2C_Read_ACK();
// Dritten Wert auslesen
MSB_Y = I2C_Read_ACK();
// Vierten Wert auslesen
LSB_Y =I2C_Read_NACK(); //Hier KEIN ACKNOWLEDGE!
I2C_Stop();
DELAY_MS(WAIT1);
// Messwerte zu 12-Bit-Zahl zusammensetzen
X_VALUE = MSB_X*256 + LSB_X;
X_VALUE = X_VALUE & 12BIT_MASK;
Y_VALUE = MSB_Y*256 + LSB_Y;
Y_VALUE = Y_VALUE & 12BIT_MASK;
if (X_VALUE > XMAX) XMAX = X_VALUE;
if (Y_VALUE > YMAX) YMAX = Y_VALUE;
if (X_VALUE < XMIN) XMIN = X_VALUE;
if (Y_VALUE < YMIN) YMIN = Y_VALUE;
Msg_WriteText("XMAX:");
Msg_WriteInt(XMAX);
Msg_WriteText("YMAX:");
Msg_WriteInt(YMAX);
Msg_WriteText("XMIN:");
Msg_WriteInt(XMIN);
Msg_WriteText("YMIN:");
Msg_WriteInt(YMIN);
Msg_WriteChar(13);
}
DRIVE_OFF();//Motoren aus
}
Buchempfehlung:
PRO-BOT128 selbst bauen und erfolgreich einsetzen
.