HDMM01 und die orientierungslib von Dirk

**inka** · 14.04.2013, 08: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, 09: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** · 14.04.2013, 10:35

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

**Dirk** · 14.04.2013, 14:28

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!

**inka** · 16.04.2013, 13:28

Zitat von Dirk

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!

habe es ein bischen (optisch) für mich angepasst

Code:

void korrekrur_richtung(void)
// Wertebereich new_dir und old_dir: 0..359
// Ergebnis in rot! Positiv: Rechtsdrehung, negativ: Linksdrehung!
//int16_t dev, rot;
// Wertebereich new_dir und old_dir: 0..359
// Ergebnis in rot! Positiv: Rechtsdrehung, negativ: Linksdrehung!
int32_t dev, rot; //weil die new und old_dir jetz auch long sind...
dev = new_dir - old_dir; //das ist die zeile 91...
rot = dev;
if (abs(dev) > 180 //fehlt da nicht eine klammer?
    {
    if (dev < 0)
    {
        rot = 360 + dev;
    }
    else
    {
        rot = -360 + dev;
    }

beim compilieren bekomme ich diese fehlermeldung:

gyro_quadrat.c|91|error: expected declaration specifiers before ‘dev’|

liegt das nur an der fehlenden(?) klammer oder an noch was anderem?

**Dirk** · 16.04.2013, 18:26

liegt das nur an der fehlenden(?) klammer oder an noch was anderem?

Ja, an den fehlenden Klammern.
Und zwar:
1. Nach void korrekrur_richtung(void) eine geschweifte KLAMMER AUF
2. Am Ende deines Ausschnitts ZWEI geschweifte KLAMMERN ZU
3. Eine runde KLAMMER ZU an der Stelle, die du markiert hast

**inka** · 16.04.2013, 19:14

Code:

{
dev = new_dir - old_dir;
rot = dev;
if (abs(dev)) > 180
{
    {
    if (dev < 0)
        {
        rot = 360 + dev;
        }
    else
        {
        rot = -360 + dev;
        }
    }
}
}

also ich glaube jetzt sind alle klammern richtig, code:blocks hat zumindest alle paarweise gefunden, trotzdem kommt bei der ersten if abfrage "abs(dev)" eine fehlermeldung:
gyro_quadrat.c|90|error: expected expression before ‘>’ token|

das mit dem daueranzeigen der werte habe ich geschafft:

Code:

case 1://richtung NORD

                        setLEDs(0b0001);                //LED 1 leuchtet
                         while(true)
                         {
                            {
                            new_dir = 180;
                            sensorwerte_holen();
                            heading_ausgeben();
                            old_dir = heading2dm;
                            setCursorPosLCD(1, 0);
                            writeStringLCD_P("dir  ");
                            writeIntegerLengthLCD(old_dir, DEC, 3);
                            writeStringLCD_P("   ");
                            writeIntegerLengthLCD(new_dir, DEC, 3);
                            mSleep(1000);
                            /*
                            if (key)
                                {
                                break;
                                }
                            */
                            }
                         }

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

                break;

nach der zweiten abfrage der variablen key (hier auskomentiert) springt das programm fast sofort (nachdem die werte einmal angezeigt wurden) wieder auf die stelle wo "button 0" angezeit wird, also vor die erste key abfrage - da wollte ich eigentlich, dass es erst auf das zweite drücken eines beliebigen buttons passiert...