HDMM01 und die orientierungslib von Dirk

**inka** · 11.04.2013, 18:29

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...

**Dirk** · 11.04.2013, 19:22

@inka:
Die Anzeige der 1/8-Richtung ist mit einem String aus 2 Zeichen gemacht.
Die Ausgabe kann man so machen:
writeStringLCD(dir);
Unabhängig davon müßte die Funktion so aufgerufen werden:
calculateDir(dir, heading2dm);

Die Funktion/Task task_I2CTWI() wird beim I2C-Master eigentlich nur gebraucht, wenn man Daten im Hintergrund vom Slave lesen möchte. In der Anleitung zum RP6 kann man das in "4.6.11.2. I²C Master" nachlesen. Da wir solche Funktionen (in der jetzigen Lib- und Demo-Version) nicht nutzen, muss diese Task nicht eingebaut werden.

**inka** · 13.04.2013, 18:09

Zitat von Dirk

Aber: Das ist ja genau eine Aufgabe als Programmer des RP6! Nehmen wir an, der Headingwert beträgt aktuell 180° (S), du möchtest aber nach Osten (90°, E) fahren. Dann must du deinem Rotate-Befehl sagen, dass der RP6 um 90° nach links rotieren muss. Danach must du wieder den Headingwert bestimmen und erneut mit Rotate korrigieren oder durch den move-Befehl den neuen Kurs halten, indem du jeweils eine Kette etwas langsamer machst, wenn eine Abweichung von deiner Zielrichtung erkannt wird.
Das ist schon eine nette Herausforderung!!

da gib ich dir uneingeschränkt recht!!

der folgender code soll bewirken, dass sie der RP6 - je nachdem welcher headingwert gemessen wurde - nach norden ausrichtet und das auf dem kürzesten weg, also nicht immer nur rechtsherum. Die headingswerte werden ausserhalb des bereiches für case1 gemessen, deshalb hier auskommentiert...

Code:

case 1://richtung NORD

                        setLEDs(0b0001);                //LED 1 leuchtet
                        {
                        /*
                        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);
                        mSleep(1000);
                        setCursorPosLCD(1, 0);
                        writeStringLCD_P("direction  ");
                        calculateDir(dir, heading2dm);  //berechnung der richtung (N,S,W,E)
                        writeStringLCD(dir);            //ausgabe der richtung
                        }
                        if (heading2dm < 180) rotate (100, 2, heading2dm, 1);
                        else rotate (100, 3, 360-heading2dm, 1);
//                        move(150, FWD, DIST_MM(500), true); //fährt 50cm
                        mSleep(1000);
//                        rotate(50, 3, 90, 1); //dreht um 90°
                        clearLCD();

ich habe auch die move befehle hier auskommentiert, ich lasse ihn erstmal nur auf der stelle drehen.

- nach links funktioniert es, auch wenn sehr ungenau - da müsste ich also die zweite messung und korrektur einbauen, das mache ich noch.

- hat die genauigkeit etwas mit der geschwindigkeit des drehens zu tun? Ich habe den eindruck, dass er, wenn er schneller ist, weit mehr übers ziel hinausgeht...und wenn er langsammer ist, das ziel nicht erreicht!

- nach rechts, also wenn der gemessener headingwert > 180° ist, dreht er sich im kreis ohne anzuhalten und ich komme nicht drauf warum!?!?

**Dirk** · 13.04.2013, 19:41

@inka:
Du must etwas mehr Aufwand treiben, um das "Problem" zu lösen.

1. Du kannst z.B. eine Variable definieren, in der du die ZIELRICHTUNG angibst, nennen wir sie int16_t new_dir .
2. Den gemessenen Heading Wert (also die aktuelle Richtung des RP6!) tun wir in eine weitere Variable: int16_t old_dir .
3. Dann muss eine "Formel" her (nicht getestet!!!), um die Abweichung zu berechnen:

Code:

int16_t temp;
temp = new_dir - old_dir;
if (new_dir < old_dir) temp += 360;

Wenn du ein Test-Prog schreibst, würde ich das auch so machen, wie du. Als Test stellt man den RP6 in eine beliebige Richtung auf und läßt ihn in die Zielrichtung (new_dir) auf der Stelle drehen. Die Variable temp kann man dann in Rotate einsetzen. Je nach Vorzeichen (+ -) dreht man nach links oder rechts.
Wenn man das gut gemacht hat, müßte sich der RP6 immer auf kürzestem Weg zur Zielrichtung drehen.

Wahrscheinlich wird ein EINMALIGES Drehen nicht zum Ziel führen, weil Rotate ziemlich ungenau ist. In deinem Prog solltest du also noch 2-3 Korrekturschleifen einbauen.

**inka** · 14.04.2013, 09:44

@dirk:
was ich in dem code nicht verstehe ist das ende der zweiten zeile: "temp += 360;
sollte nicht je nach ergebnis der if abfrage 360 abgezogen, oder dazugezählt werden? Also + oder -?

**Dirk** · 14.04.2013, 10:06

@inka:
Ja, kann sein, dass die Formel so noch nicht stimmt! Ich habe sie nur nur mal schnell zusammen geschustert und nicht geprüft.
Am besten machst du dir mal ein Testprog, bei dem du Ziel- und Ausgangsrichtung eingeben kannst und das dir die korrekte Drehrichtung ausgibt.

**inka** · 15.04.2013, 10:49

hi Dirk,

alle änderungen ausgeführt, beim ersten drücken des buttons 1 stimmt die ausgabe (diesmal dir_neu = 360), beim 2ten drücken immer noch 256...

Code:

#include "RP6ControlLib.h"
#include "RP6I2CmasterTWI.h"
#include "RP6Control_OrientationLib.h"
//#include "RP6Control_I2CMasterLib.h"
#include "RP6Control_MultiIOLib.h"

#define I2C_RP6_BASE_ADR 10

uint8_t ch;
char item[12];
char dir[3];
int16_t new_dir; //neue richtung
int16_t old_dir; //gemessene richtung
int16_t temp; //berechnung korrektur richtung

/*
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);
}

/*******************sensorwerte holen*************************/
void sensorwerte_holen(void)
{
    intreg2dm = readHDMM01();        // holt sersorwerte
    if  (!intreg2dm)                //daten gültig?
    {
        normalizeHDMM01();              // daten werden "normalisiert"
        heading2dm = headingHDMM01(); // berechnung der headingwerte
    }
}

/******************heading auf LCD schreiben**************/
void heading_ausgeben(void)
{
        setCursorPosLCD(0, 0);          //headingwerte werden auf LCD geschrieben
        writeStringLCD_P("Heading   ");
        writeIntegerLCD(heading2dm, DEC);
}

/*******************berechnung der richtungswerte***********/
void richtung_berechnen(void)
{
   calculateDir(dir, heading2dm);  //berechnung der richtung (N,S,W,E)
}

/**************ausgabe der richtungswerte*******************/
void richtung_ausgeben(void)
{
        setCursorPosLCD(1, 0);
        writeStringLCD_P("direction  ");
        writeStringLCD(dir);            //ausgabe der richtung
}



int main(void)
{


initRP6Control();
multiio_init();
initLCD();
orientation_init();
//COMPASS_2D_init();

    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_quadrat");
    mSleep(2500);
    clearLCD();



    while(true)
    {

/*****************anzeige gedrückter buttons****************/
        clearLCD();
        pressedMultiIOButtonNumber = getMultiIOPressedButtonNumber();
        setCursorPosLCD(0, 0);
        writeStringLCD("Button: ");
        writeIntegerLCD(pressedMultiIOButtonNumber, DEC);
        mSleep(500);

        uint8_t key = getMultiIOPressedButtonNumber();


/********************funktion der buttons*********************/
        if(key)
        {
            switch(key)
            {
                case 1://richtung NORD

                        setLEDs(0b0001);                //LED 1 leuchtet
                        {
                            new_dir = 360;
                            sensorwerte_holen();
                            heading_ausgeben();
                            old_dir = heading2dm;
                            setCursorPosLCD(1, 0);
                            writeStringLCD_P("dir  ");
                            writeIntegerLCD(old_dir, DEC);
                            writeStringLCD_P(" ");
                            writeIntegerLCD(new_dir, DEC);
                            mSleep(1000);
                        }

//                        move(150, FWD, DIST_MM(500), true); //fährt 50cm
                        mSleep(1000);
//                        rotate(50, 3, 90, 1); //dreht um 90°
                        clearLCD();

                break;
                case 2:
                    setLEDs(0b0010);

                break;
                case 3:
                    setLEDs(0b0100);
                break;
                case 4:
                    setLEDs(0b1000);
                break;

            }
        }


    }


    return 0;
}

irgendwie haben die 256 mit der definition der variablen zu tun, aber wie?

**inka** · 15.04.2013, 18:17

@Dirk:

ich habe jetzt die 3 variablen old_dir, new_dir und temp von "int16_t" in "int32_t" geändert und nun ist die ausgabe richtig!

Soll ich es nun so lassen (weil es funktioniert) oder gibt es eine alternative zu "long"...(und evtl. eine erklärung die ich verstehen würde?)