HDMM01 und die orientierungslib von Dirk
hi allerseits,
ich habe mir ein kleines progrämmchen zusammengestöpselt
Code:
#include "RP6ControlLib.h"
#include "RP6I2CmasterTWI.h"
#include "RP6Control_OrientationLib.h"
#include "RP6Control_I2CMasterLib.h"
#define I2C_RP6_BASE_ADR 10
uint8_t ch;
char item[12];
char dir[3];
uint16_t heading;
void I2C_requestedDataReady(uint8_t dataRequestID) //macht was?
{
checkRP6Status(dataRequestID);
}
void calculateDir(char* dir, uint16_t heading) //setzt headingwerte grob in himmelsrichtungen um
{
dir[1] = ' ';
dir[2] = '\0';
if ((heading <= 22) || (heading >=338)) dir[0] = 'N';
if ((heading >= 23) && (heading <= 67)) {dir[0] = 'N'; dir[1] = 'E';}
if ((heading >= 68) && (heading <= 112)) dir[0] = 'E';
if ((heading >= 113) && (heading <= 157)) {dir[0] = 'S'; dir[1] = 'E';}
if ((heading >= 158) && (heading <= 202)) dir[0] = 'S';
if ((heading >= 203) && (heading <= 247)) {dir[0] = 'S'; dir[1] = 'W';}
if ((heading >= 248) && (heading <= 292)) dir[0] = 'W';
if ((heading >= 293) && (heading <= 337)) {dir[0] = 'N'; dir[1] = 'W';}
}
void I2C_transmissionError(uint8_t errorState) //gibt I2C fehlermeldungen über LCD aus
{
clearLCD();
writeStringLCD_P("I2C ERROR -->");
setCursorPosLCD(1, 0); // line 2
writeStringLCD_P("TWI STATE: 0x");
writeIntegerLCD(errorState, HEX);
}
int main(void)
{
initRP6Control();
initLCD();
setLEDs(0b1111);
mSleep(500);
setLEDs(0b0000);
I2CTWI_initMaster(100);
I2CTWI_setRequestedDataReadyHandler(I2C_requestedDataReady);//macht was?
I2CTWI_setTransmissionErrorHandler(I2C_transmissionError); //aktiviert I2C fehlermeldungen
showScreenLCD(" RP6Control M32", " gyro_test_2");
mSleep(2500);
clearLCD();
orientation_init();
while(true)
// task_I2CTWI();
{
task_I2CTWI();
intreg2dm = readHDMM01(); // holt sersorwerte
if(!intreg2dm)
{ // daten gültig?
normalizeHDMM01(); // daten werden "normalisiert"
heading2dm = headingHDMM01(); // berechnung der headingwerte
setCursorPosLCD(0, 0);
writeStringLCD_P("Heading ");
writeIntegerLCD(heading2dm, DEC);
setCursorPosLCD(1, 0);
writeStringLCD_P("direction ");
calculateDir(dir, heading); //berechnung der richtung (N,S,W,E)
writeCharLCD(dir[0]); //ausgabe der ersten stelle des richtungswertes - es wird grundsätzlich nur "N" ausgegeben...
// writeCharLCD(dir[1]); //ausgabe der 2ten stelle des richtungswertes -funktioniert nicht...
}
// move(60, 0, 1000, 1);
move(150, FWD, DIST_MM(500), true); //fährt 50cm
mSleep(300);
rotate(50, 3, 90, 1); //wendet um 90°
clearLCD();
task_I2CTWI(); //macht was?
}
return 0;
}
es macht nicht viel: fährt im viereck (ungefähr) und gibt auf dem LCD die headingwerte des 2D kompasses aus.
Es werden auch noch die himmelsrichtungen berechnet, die ausgabe funktioniert aber schon mal nicht korrekt: es wird grundsätzlich nur ein "N" ausgegeben. Diese "himmelsrichtungen" sind zwar nicht wichtig, für die bestimmung und änderung der fahrtrichtung sind sie zu grob, aber für die ausgabe am display wäre es für mich schon wichtig was da nicht stimmt...
im weiteren verlauf möchte ich folgendes versuchen zu realisieren:
- fahren in bestimmte himmelsrichtungen, evtl. nach abfrage der taster oder anderen vorgaben
- richtungswechsel nach der erkennung eines hindernises
- abfrage der momentaner fahrtrichtung und korrektur mit taster li/re oder um eine gradzahl, (per RC?)
im moment verstehe ich vieles im code nicht, deshalb habe ich als kommentare fragen eingefügt, wäre schön, wenn mir jemand ein paar tipps zum verständnis geben könnte...
