naja

es ist das beispielprogramm i2c_slave von der lib des rp6

und es ist das beispiel prgramm movement2 vom c controller


Movement:
Code:
/*******************************************************************************
 * RP6 C-Control PRO M128 Erweiterungsmodul
 * ----------------------------------------------------------------------------
 * I2C Bus - Bewegung & Sensorik - Beispielprogramm
 * ----------------------------------------------------------------------------
 * Ein weiteres einfaches Bewegungsprogramm, diesmal reagiert der RP6
 * allerdings auf die Bumper.
 * Zunächst fährt der RP6 vorwärts in einer Kreisbahn.
 * Kollidiert er mit einem Hindernis, werden die Motoren gestoppt und
 * die LEDs fangen an zu blinken.
 * (man könnte natürlich auch anders auf die Kollision reagieren...)
 ******************************************************************************/


#include "../../RP6CCLib/RP6CCLib.cc"

// ###########################
#include "RP6MasterLib.cc"

// ###########################
#include "RP6_Sensorcheck.cc"

/**
 * Dieser Thread wird ausgeführt sobald der Roboter mit einem
 * Hindernis kollidiert ist.
 */
void blink(void)
{
    while(true)
    {
        setRP6LEDs(LED2 | LED3);
        setLEDs(LED3 | LED4 | LED5);  // LEDs kurz aufblitzen lassen ...
        Thread_Delay(5);              // ...
        setLEDs(0);                   // ...
        Thread_Delay(30);
        setRP6LEDs(LED5 | LED6);
        setLEDs(LED3 | LED4 | LED5);
        Thread_Delay(5);
        setLEDs(0);
        Thread_Delay(30);
    }
}


void main(void)
{
	RP6_CCPRO_Init(); // Auf Startsignal warten!

    newline();
    println("   ________________________");
    println("   \\| RP6  ROBOT SYSTEM |/");
    println("    \\_-_-_-_-_-_-_-_-_-_/ ");
    newline();
    println("  RP6 Movement DEMO 2 ");
    newline();

    beep(100, 200, 300);

    showScreenLCD("Movement", "DEMO Program 2");
    AbsDelay(2000);
    clearLCD();

    // ACS auf mittlere Sendeleistung stellen:
    RP6_writeCMD_1param(RP6_BASE_ADR, CMD_SET_ACS_POWER, ACS_PWR_MED);
    RP6_writeCMD_1param(RP6_BASE_ADR, CMD_SET_WDT, true);
    RP6_writeCMD_1param(RP6_BASE_ADR, CMD_SET_WDT_RQ, true);

    // Der Thread zur Überprüfung von Statusänderungen wird gestartet:
    Thread_Start(1,thread_checkINT0);

    // Vorwärts im Kreis fahren:
    changeDirection(FWD);
    moveAtSpeed(100,40);

    setCursorPosLCD(1,0);
    printLCD("Moving...    :-)");

    setRP6LEDs(LED1 | LED4);

    byte stopped;
    stopped = false;

    while(true)
    {
        if(!stopped) { // Es sollte nur einmal stop() und Thread_Start aufgerufen
                       // werden, daher fragen wir hier ab, ob das bereits
                       // geschehen ist.

            // Man muss die Bumper nicht in RP6_Sensorcheck.cc
            // auswerten. Man kann auch hier direkt die beiden Variablen
            // der Tastsensoren abfragen:
            if(bumperLeft || bumperRight)
            {
                stop();                 // Roboter stoppen!
                Thread_Start(2,blink);  // Blinken!
                stopped = true;  // Roboter wurde angehalten!
                setCursorPosLCD(1,0);
                printLCD("Ouch! Crash! :-(");
            }
        }
        Thread_Delay(1); // Muss immer hier rein, sonst werden die anderen
                         // Threads blockiert.
    }
}
sensorcheck:
Code:
 /*******************************************************************************
 * RP6 C-Control PRO M128 Erweiterungsmodul
 * ----------------------------------------------------------------------------
 * I2C Bus Beispielprogramm
 * ----------------------------------------------------------------------------
 * In diese Datei wurde die Sensorauswertung ausgelagert. So bleibt
 * das Programm insgesamt übersichtlicher.
 *
 * Die Ausgaben des ACS wurden in diesem Beispiel entfernt!
 ******************************************************************************/

//------------------------------------------------------------------------------
//

#ifndef __SENSORCHECK__     // Alles nur EINMAL einbinden!
#define __SENSORCHECK__
// Diese zwei Zeilen und die letzte in dieser Datei sind wichtig, sonst
// funktioniert das übersetzen nicht! Auch bei eigenen Erweiterungen MUSS das
// am Anfang stehen!

/**
 * Wenn sich der Zustand des ACS verändert hat, wird diese Funktion
 * aufgerufen. Was hier genau gemacht wird, hängt dann von der Anwendung ab.
 */
void handle_ACS(void)
{

}

/**
 * Genauso wie bei der ACS Funktion, wird handle_Bumpers aufgerufen sobald
 * sich der Zustand der Tastsensoren vom RP6 geändert hat.
 */
void handle_Bumpers(void)
{
    beep_t(100, 1, 1);

    if(bumperRight) {    // Rechter Tastsensor
        setCursorPosLCD(0,14);
        printLCD("BR");
    }
    else  // Links nichts detektiert - also Text vom LCD löschen:
        clearPosLCD(0,14,2);

    if(bumperLeft) {  // Linker Tastsensor
        setCursorPosLCD(0,0);
        printLCD("BL");
    }
    else
        clearPosLCD(0,0,2);

    if(bumperLeft && bumperRight) {  // Beide, also in der Mitte
        setCursorPosLCD(0,6);
        printLCD("MID");
    }
    else
        clearPosLCD(0,6,3);
}

/**
 * Bei niedriger Akkuspannung wird diese Funktion aufgerufen
 */
void handle_BatLow(void)
{
    print("BAT LOW:");
    printInteger(adcBat);
    println(" ");
}

/**
 * Sobald Bewegungsabläufe abgeschlossen sind, oder bei einer
 * Fehlerabschaltung aufgrund blockierter Motoren wird diese Funktion
 * aufgerufen.
 */
void handle_DriveSystemChange(void)
{
    print(" DRIVE:");
    printInteger(movementComplete);
    printInteger(motorOvercurrent);
    println(" ");
}


/**
 * Der RP6 kann mit dem IR Empfänger vom ACS auch RC5 Codes von Fernbedienungen
 * empfangen. Sobald ein Tastencode empfangen wurde, wird diese Funktion
 * aufgerufen.
 */
void handle_RC5_reception(void)
{
    beep_t((RC5_data+5)*15, 2, 2);
    print(" RC5: ");
    printInteger(RC5_toggle);   // Togglebit (Taste gehalten
    print(":");                 // oder mehrfach gedrückt?)
    printInteger(RC5_adr);      // Adresse
    print(":");
    printInteger(RC5_data);     // Tastencode
    println(" ");
}

/**
 * Damit der Roboter nicht ausser Kontrolle gerät, wenn das CCPRO oder andere
 * Host Controller abstürzen (oder die Kommunikation aus sonstigen Gründen
 * gestört ist), kann ein Watchdog Timer aktiviert werden, der
 * alle 250ms eine Anfrage an den Host sendet. Antwortet dieser nicht innerhalb
 * von 3 Sekunden auf diese Anfrage, werden die Motoren gestoppt und der Roboter
 * muss zurückgesetzt werden bevor er weiterfahren kann.
 *
 * Man kann diese periodischen Anfragen auch gut verwenden um zu signalisieren
 * dass das Programm noch läuft, indem man eine LED blinken lässt, oder wie
 * in diesem Beispielprogramm ein Zeichen auf dem LC-Display ("Heartbeat").
 */
byte heartbeat;
void handle_WDT_RQ(void)
{
    if(heartbeat)
    {
        clearPosLCD(0, 11, 1);
        heartbeat = false;
    }
    else
    {
        setCursorPosLCD(0,11);
        printLCD("*");
        heartbeat = true;
    }
}

/**
 * Dies ist die etwas aufgeräumte Interrupt Behandlungsroutine. Die eigentliche
 * Auswertung der Daten erfolgt in den einzelnen Funktionen oben. Diese
 * werden hier bei Bedarf aufgerufen.
 */
void thread_checkINT0(void)
{
    while(true) {
        if(Port_ReadBit(PORT_PE5_INT)) { // Hat der Controller auf dem Mainboard neue Daten?
            // Die drei Status Register auslesen:
            RP6_readRegisters(RP6_BASE_ADR, 0, messageBuf, 3);

            // Wir überprüfen, ob die Status Bits sich verändert haben,
            // also verknüpfen wir den alten und neuen Wert per Exclusiv Oder
            // um nur die geänderten Bits auswerten zu müssen.
            byte compare;
            compare = messageBuf[0] ^ interrupt_status;

            interrupt_status = messageBuf[0];

            // ------------------------------------
            // Zunächst prüfen wir ob sich die ACS Status Bits verändert haben.
            // Falls nicht, hat irgend etwas anderes den Interrupt ausgelöst und
            // wir können mit den anderen Abfragen weitermachen.
            if(compare & MASK_ACS)
            {
                 // Die ACS Status Variablen setzen:
                 obstacleLeft = messageBuf[0] & 0x20;
                 obstacleRight = messageBuf[0] & 0x40;

                 // ACS in getrennter Funktion weiterbehandeln:
                 handle_ACS();
            }

            // ------------------------------------
            // Abfragen ob sich der Zustand der Bumper geändert hat, falls
            // ja, Text auf der seriellen Schnittstelle ausgeben:
            if(compare & MASK_BUMPER)
            {
                 bumperLeft = messageBuf[0] & 0x2;
                 bumperRight = messageBuf[0] & 0x4;

                 handle_Bumpers();
            }

            // ------------------------------------
            // Überprüfen ob ein RC5 Datenpaket mit dem IRCOMM empfangen
            // worden ist - falls ja auf der seriellen ausgeben:
            if(interrupt_status & MASK_RC5)
            {
                RP6_readRegisters(RP6_BASE_ADR, I2C_REG_RC5_ADR, messageBuf, 2);
                RC5_toggle = messageBuf[0] >> 5;
                RC5_adr = messageBuf[0] & 0x1F;
                RC5_data = messageBuf[1];

                handle_RC5_reception();
            }

            // ------------------------------------
            // Überprüfen ob die Batteriespannung zu niedrig ist:
            if(compare & MASK_BATLOW)
            {
                RP6_readRegisters(RP6_BASE_ADR, I2C_REG_ADC_UBAT_L, messageBuf, 2);
                adcBat = (messageBuf[1] << 8) | (messageBuf[0]);

                handle_BatLow();
            }

            // ------------------------------------
            // Bewegung fertig? Notabschaltung?
            if(compare & MASK_DRIVE)
            {
                 movementComplete = messageBuf[2] & 1;
                 motorOvercurrent = messageBuf[2] & 4;
                 handle_DriveSystemChange();
            }

            // ------------------------------------
            // Auf Watchdog Requests prüfen:
            if(messageBuf[1] & MASK_WDT_RQ)
            {
                handle_WDT_RQ();
            }
        }

        Thread_Delay(1);
    }
}

#endif