HDMM01 und die orientierungslib von Dirk

@inka:
Die Long-Variablen brauchst du eigentlich nicht. Insofern verstehe ich nicht, warum das so dann läuft.

irgendwie beruhigend :-) gruss inka

Zitat:

---

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?

Zitat:

---

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

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

Hallo,
bei

Zitat:

---

if (abs(dev)) > 180

---

sollte das > 180 mit in die Klammer: if (abs(dev) > 180)
Thorben

@inka:

Zitat:

---

bei der ersten if abfrage "abs(dev)" eine fehlermeldung: gyro_quadrat.c|90|error: expected expression before ‘>’ token|

---

Die Zeile müßte so aussehen:
if (abs(dev) > 180)

Zitat:

---

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

---

Willst du denn dort, wo die key-Abfrage auskommentiert ist, die Tastatur abfragen? Wenn ja, müßtest du eine der Funktionen da einfügen, mit denen die Tastatur abgefragt wird, z.B.: key = getMultiIOPressedButtonNumber();

@Dirk:

das mit dem kontiniuerlichen anzeigen der werte und dem abbruch per zweiten tastendruck funktioniert jetzt so weit :-)

die fuktion:

Code:

---

void korrekrur_richtung(void)
// Wertebereich new_dir und old_dir: 0..359
// Ergebnis in rot! Positiv: Rechtsdrehung, negativ: Linksdrehung!
{
dev = new_dir - old_dir;
rot = dev;
if (abs(dev) > 180)
{
    {
    if (dev < 0)
        {
        rot = 360 + dev;
        }
    else
        {
        rot = -360 + dev;
        }
    }
}
}

---

berechnet und legt in der variablen "rot" den wert ab und die richtung der drehung sollte durch das vorzeichen von "rot" bestimmt werden. In der funktion:

void rotate(uint8_t desired_speed, uint8_t dir, uint16_t angle, uint8_t blocking)

werden ja die parameter speed, dir, angle und blocking eingesetzt und ausgeführt. Der parameter für die drehrichtung ist aber per vorzeichen schon in der variablen "rot" enthalten. Wie "trixe" ich nun den befehl rotate aus?

@inka:

Zitat:

---

Wie "trixe" ich nun den befehl rotate aus?

---

Die Parameter desired_speed und blocking sind ja klar.
Für angle setzt du abs(rot) ein.
Für den Parameter dir testest du rot auf das Vorzeichen:
uint8_t rdir;
if (rot < 0) rdir = LEFT;
else rdir = RIGHT;

Dann kannst du rotieren:
rotate(80, rdir, (abs(rot)), true);

also, das programm tut "im großen und ganzen" in den bereichen die mir wichtig waren was es soll. Ich habe den ganzen code hier noch einmal kopiert, weil einzelne abschnitte sicher nicht so übersichtlich wären, es ist sicher noch eine ganze menge "müll" drinnen, auch viel zu viele "mSleeps", aber ich wollte mitlesen auf dem display...

Code:

---

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

#define I2C_RP6_BASE_ADR 10

uint8_t ch;
char item[12];
char dir[3];
int32_t new_dir; //neue richtung
int32_t old_dir; //gemessene richtung
int32_t temp; //berechnung korrektur richtung
int16_t dev, rot, i;//berechnung rotation
char rdir;

/***********************calculate_dir**********************/
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
    }
}

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

/*************korrektur richtung*****************************/

void korrekrur_richtung(void)
// Wertebereich new_dir und old_dir: 0..359
// Ergebnis in rot! Positiv: Rechtsdrehung, negativ: Linksdrehung!
{
dev = new_dir - old_dir;
rot = dev;
if (abs(dev) > 180)
{
    {
    if (dev < 0)
        {
        rot = 360 + dev;
        }
    else
        {
        rot = -360 + dev;
        }
    }
}
}
/*******************werte schreiben************************/

void werte_schreiben(void)
{
    setCursorPosLCD(0, 0);
    writeStringLCD_P("HEA ODR NDR  ROT");
    old_dir = heading2dm;
    setCursorPosLCD(1, 0);
    writeIntegerLengthLCD(heading2dm, DEC, 3);
    setCursorPosLCD(1, 4);
    writeIntegerLengthLCD(old_dir, DEC, 3);
    setCursorPosLCD(1, 8);
    writeIntegerLengthLCD(new_dir, DEC, 3);
    korrekrur_richtung();
      if (rot<0)
      {
      setCursorPosLCD(1, 12);
      writeStringLCD_P("-");
      }
      else
      {
      setCursorPosLCD(1, 12);
      writeStringLCD_P(" ");
      }
    setCursorPosLCD(1, 13);
    writeIntegerLengthLCD(abs(rot), DEC, 3);
}

/**********************test rdir**************************/
void test_rdir(void)
{
if (rot < 0)
rdir = LEFT;
else rdir = RIGHT;
}

/**********************hauptprogramm*********************/

int main(void)
{


initRP6Control();
multiio_init();
initLCD();
orientation_init();


    setLEDs(0b1111);
    mSleep(500);
    setLEDs(0b0000);

I2CTWI_initMaster(100);
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
                        while(true)
                        {
                            {
                            new_dir = 360;
                            sensorwerte_holen();
                            werte_schreiben();
                            mSleep(1000);
                            korrekrur_richtung();
                            test_rdir();
                            werte_schreiben();
                            mSleep(1000);
                            rotate(60, rdir, ((abs(rot)/2)), 0);
                            werte_schreiben();
                            mSleep(1000);
//                            if ((rot < 2) || (rot > -2)) break;
//                            if (rot == 0) break;

                            for (i= 0; i<3; i++);
                            {
                            sensorwerte_holen();
                            werte_schreiben();
                            mSleep(1000);
                            korrekrur_richtung();
                            test_rdir();
                            werte_schreiben();
                            mSleep(1000);
                            }

                            uint8_t key = getMultiIOPressedButtonNumber();
                            if (key)
                                {
                                break;
                                }

                            }
                        }

                break;
                case 2:// richtung OST
                    setLEDs(0b0010);

                break;
                case 3:// richtung SÜD
                    setLEDs(0b0100);

                break;
                case 4:// richtung WEST
                    setLEDs(0b1000);

                break;

            }
        }


    }


    return 0;
}

---

erste frage:

der RP6 arbeitet sich durch den halbierten "rotate-satz" recht langsam an sein ziel, hier die nordrichtung, heran, tänzelt dann so in meistens immer kleineren schritten hin und her auf die "0" hin, die drehungen fallen aber auch manchmal völlig unerwartet größer aus. So werden aus 3° plötzlich wieder 7° - und ich weiss nicht warum. Liegt das nur an der ungenauigkeit des getriebes?

zweite frage:

er schafft auch schon mal die "0" zu erreichen, die erscheint dann auch im display, statt aber - wie ihm das "break" (jetzt auskomentiert) eigentlich befehlen sollte - sofort stehen zu bleiben, dreht er noch um 7, manchmal aber um bis zu 9° nach rechts. Und das ist verifizierbar...
Da bin ich auch noch am rätseln welche wariable mir dort einen streich spielt...

ansonsten muss ich wieder die arbeit an der platine und der lib loben, an mein "gebastle" in der vergangenheit darf ich gar nicht denken. Sauber...