HDMM01 und die orientierungslib von Dirk

**inka** · 02.05.2013, 15:06

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

**Dirk** · 02.05.2013, 17:04

Hi inka:
Glückwunsch zum funktionierenden Programm! Hoffentlich wird deinem RP6 nicht schwindelig vom vielen Drehen!

zu 1 und 2)
Wenn die etwas größeren "Sprünge" nicht durch tatsächliche "Ausreißer" einerseits des Kompass-Richtungswerts, andererseits durch ungenaues Drehen auf der Stelle, verursacht werden (das würdest du ja in deiner Werte-Ausgabe merken!), dann kann das Phänomen auch daran liegen, dass du ja "nicht-blockierend" drehst, d.h. dass sich Drehbefehle "überlappen", weil bei einem neuen Drehbefehl noch evtl. der alte aktiv ist.

**inka** · 03.05.2013, 09:56

hi Dirk,

schwindelig sicher nicht, er dreht ja hin und her

, allerdings beim ändern des blocking-parameters dreht er nur einmal und bleibt stehen. Im handbuch:

Code:

An diesem Beispiel sieht man, wie die beiden Funktionen benutzt werden können:

#include "RP6RobotBaseLib.h"
int main(void)
{
initRobotBase();
setLEDs(0b111111);
mSleep(1500);
powerON(); // Encoder und Motorstromsensoren anschalten!
}
while(true)
{
setLEDs(0b100100);
move(60, FWD, DIST_MM(300), true); // 30cm Vorwärts fahren
setLEDs(0b100000);
rotate(50, LEFT, 180, true); // um 180° nach links drehen
setLEDs(0b100100);
move(60, FWD, DIST_MM(300), true); // 30cm Vorwärts fahren
setLEDs(0b000100);
rotate(50, RIGHT, 180, true); // um 180° nach rechts drehen
}
return 0;

Hier fährt der Roboter zunächst 30cm Vorwärts, dreht dann um 180° nach links, fährt die 30cm zurück, 
dreht um 180° nach rechts und das Spiel beginnt von neuem.

Würde man den blocking Parameter von allen Funktionen false anstatt true setzen, 
würde das Programm überhaupt nicht funktionieren, da aus der Main Funktion nicht die task_motionControl Funktion 
aufgerufen wird und alle Funktionsaufrufe direkt hintereinander stehen.

Ändert man auch nur einen der blocking Parameter in false um, wird das Programm ebenfalls nicht mehr
 wie gewünscht funktionieren – es wird dann eine der Bewegungsphasen komplett übersprungen.

Für solche rein sequentiellen Abläufe muss man den blocking Parameter also immer auf true setzen!

das würde den abbruch der drehungen nach der ersten drehung erklären, es wird auch wieder die "task_motionControl Funktion" erwähnt. Wird die automatisch in dem base-slave-programm aufgerufen?

**Dirk** · 03.05.2013, 10:28

Hi inka,
nein, die "task_motionControl Funktion" brauchst du nicht, wenn du mit der M32 als I2C-Master die Base ansteuerst.
Der Grund, warum das rotate blockierend nur 1x ausgeführt wird, liegt wohl daran, dass dein rotate-Block innerhalb der if(key) Abfrage liegt.
Du müßtest dein Programm so umschreiben, dass die Richtungsmessung, -korrektur und (blockierendes) rotate in der while(true)-Schleife unabhängig vom Tastendruck immer durchlaufen kann bis zu einer Abbruchbedingung (z.B. +- 5° vom Zielwert).
Die Tastaturabfrage würde dann parallel laufen und nur ausgewertet, wenn die "richtige" Taste gedrückt wird.

**inka** · 04.05.2013, 12:34

hi Dirk,

ich kann mir vorstellen das programm so umzuschreiben dass die abfrage der 4 tasten (himmelsrichtungen) bestandteil von der while(true) schleife ist, also umgekehrt zu derm wie es jetzt ist. Ist es das was du meinst?

Den letzten satz mit der paralellel-lauf und abfrage der tasten - wie meinst du das mit dem parallelem lauf?

**Dirk** · 04.05.2013, 13:26

Hi inka,
vergiß es einfach. Ich hatte nicht gesehen, dass du innerhalb der Haupt-while(true)-Schleife noch eine 2. while(true)-Schleife hast, in der dann die Drehung stattfindet. So kann man es natürlich auch gut machen.