-         

Seite 1 von 6 123 ... LetzteLetzte
Ergebnis 1 bis 10 von 52

Thema: HDMM01 und die orientierungslib von Dirk

  1. #1
    Erfahrener Benutzer Roboter Genie Avatar von inka
    Registriert seit
    29.10.2006
    Ort
    nahe Dresden
    Alter
    70
    Beiträge
    1.344

    HDMM01 und die orientierungslib von Dirk

    Anzeige

    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...
    gruß inka

  2. #2
    Erfahrener Benutzer Robotik Einstein Avatar von Dirk
    Registriert seit
    30.04.2004
    Ort
    NRW
    Beiträge
    3.791
    @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.
    Geändert von Dirk (11.04.2013 um 19:31 Uhr)
    Gruß
    Dirk

  3. #3
    Erfahrener Benutzer Roboter Genie Avatar von inka
    Registriert seit
    29.10.2006
    Ort
    nahe Dresden
    Alter
    70
    Beiträge
    1.344
    Zitat Zitat von Dirk Beitrag anzeigen
    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!?!?
    Geändert von inka (13.04.2013 um 18:18 Uhr)
    gruß inka

  4. #4
    Erfahrener Benutzer Robotik Einstein Avatar von Dirk
    Registriert seit
    30.04.2004
    Ort
    NRW
    Beiträge
    3.791
    @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.
    Gruß
    Dirk

  5. #5
    Erfahrener Benutzer Roboter Genie Avatar von inka
    Registriert seit
    29.10.2006
    Ort
    nahe Dresden
    Alter
    70
    Beiträge
    1.344
    @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 -?
    gruß inka

  6. #6
    Erfahrener Benutzer Robotik Einstein Avatar von Dirk
    Registriert seit
    30.04.2004
    Ort
    NRW
    Beiträge
    3.791
    @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.
    Gruß
    Dirk

  7. #7
    Erfahrener Benutzer Roboter Genie Avatar von inka
    Registriert seit
    29.10.2006
    Ort
    nahe Dresden
    Alter
    70
    Beiträge
    1.344
    @dirk:

    ein weiteres phänomen, für mich völlig unerklärlich:

    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];
    uint16_t new_dir; //neue richtung
    uint16_t old_dir; //gemessene richtung
    uint16_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 = 257;
                                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;
    }
    im case1:

    das programm startet normal, die ausgabe "button 0" erscheint, nach dem drücken des button 1 erscheinen alle ausgaben:

    "heading" in erster zeile, "old_dir" und "new_dir" in der zweiten - auch mit den richtigen werten.

    beim 2ten mal drücken des button 1 erscheint aber der wert von "new_dir" entweder als "0", wenn er vorher auf einen wert bis 256 festgelegt wurde, alles darüber erscheint als "256".

    verstehe ich echt nicht....
    gruß inka

  8. #8
    Erfahrener Benutzer Robotik Einstein Avatar von Dirk
    Registriert seit
    30.04.2004
    Ort
    NRW
    Beiträge
    3.791
    Ich habe mal ein paar kleinere Änderungen gemacht. Ich habe das NICHT geprüft, weil ich die M32 nicht an die MultiIO angeschlossen habe:
    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];
    //uint16_t new_dir; //neue richtung
    //uint16_t old_dir; //gemessene richtung
    //uint16_t temp; //berechnung korrektur richtung
    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? <=== KLAMMER AUF!!!
             normalizeHDMM01();                // daten werden "normalisiert"
             heading2dm = headingHDMM01();    // berechnung der headingwerte
        }    // <=== KLAMMER ZU!!!
    }
    
    /******************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();
            uint8_t key = pressedMultiIOButtonNumber;
    
    
    /********************funktion der buttons*********************/
            if(key)
            {
                switch(key)
                {
                    case 1://richtung NORD
    
                            setLEDs(0b0001);                //LED 1 leuchtet
                            {
                                new_dir = 257;
                                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;
    }
    Schau dir mal die Änderungen an:
    1. RP6Control_I2CMasterLib nicht eingebunden
    2. Variablen new_dir, old_dir und temp als int16_t
    3. I2C_requestedDataReady weg
    4. In sensorwerte_holen() KLAMMERN ergänzt
    5. COMPASS_2D_init() weg (wird in der Lib aufgerufen)
    6. I2CTWI_setRequestedDataReadyHandler() weg
    7. getMultiIOPressedButtonNumber() nicht 2x aufgerufen, weil die Funktion nur EINMAL die gedrückte Taste ausgibt

    Du must gucken, ob das jetzt ok ist. Ein allg. Problem ist sicher, dass die Ausgabe nur 1x erfolgt, nachdem du gedrückt hast. Vielleicht änderst du das z.B. noch so, dass die Ausgabe erfolgt, SOLANGE keine andere Taste als die 1 gedrückt wurde.

    Um die Rotation zu errechnen, um von old_dir zu new_dir zu kommen, habe ich jetzt auch eine passende Formel:
    Code:
    // Wertebereich new_dir und old_dir: 0..359
    // Ergebnis in rot! Positiv: Rechtsdrehung, negativ: Linksdrehung!
    int16_t dev, rot;        
    dev = new_dir - old_dir;        
    rot = dev;        
    if (abs(dev) > 180) {        
        if (dev < 0) {    
            rot = 360 + dev;
        }    
        else {    
            rot = -360 + dev;
        }    
    }
    Der Drehwinkel zu new_dir ist danach in rot, wobei positive Werte für Rechtsdrehung stehen und negative für Linksdrehung.
    Du must also nur den Absolutwert von rot an die rotate-Funktion übergeben und je nach Vorzeichen links oder rechts drehen lassen.
    Teste das mal!
    Geändert von Dirk (16.04.2013 um 19:46 Uhr) Grund: Runde Klammer fehlte im Code
    Gruß
    Dirk

  9. #9
    Erfahrener Benutzer Roboter Genie Avatar von inka
    Registriert seit
    29.10.2006
    Ort
    nahe Dresden
    Alter
    70
    Beiträge
    1.344
    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?
    gruß inka

  10. #10
    Erfahrener Benutzer Roboter Genie Avatar von inka
    Registriert seit
    29.10.2006
    Ort
    nahe Dresden
    Alter
    70
    Beiträge
    1.344
    @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?)
    gruß inka

Seite 1 von 6 123 ... LetzteLetzte

Ähnliche Themen

  1. Kompass-Modul Hdmm01 für den Rp6
    Von Morpheus1997 im Forum Robby RP6
    Antworten: 8
    Letzter Beitrag: 09.08.2012, 18:33
  2. 2 x HDMM01 auf Arduino Mega
    Von arnoa im Forum Sensoren / Sensorik
    Antworten: 2
    Letzter Beitrag: 08.02.2012, 17:19
  3. Pollin I2C Kompassmodul HDMM01
    Von malthy im Forum Sensoren / Sensorik
    Antworten: 11
    Letzter Beitrag: 15.09.2011, 13:53
  4. Zu Servo anssteuerung von Dirk frage
    Von Christian3 im Forum Robby RP6
    Antworten: 1
    Letzter Beitrag: 16.06.2009, 13:31
  5. @Dirk Gemeinsames Modul
    Von UweMD im Forum Robby CCRP5
    Antworten: 2
    Letzter Beitrag: 21.09.2004, 07:21

Berechtigungen

  • Neue Themen erstellen: Nein
  • Themen beantworten: Nein
  • Anhänge hochladen: Nein
  • Beiträge bearbeiten: Nein
  •