File content as of revision 4:a975caedface:

/***********************************
* name:   ur_Karel    v 0.2
* author: PE HTL BULME
* email:  pe@bulme.at
* description:
*    functions for Karel The Robot
*
*************************************/
#include "mbed.h"
#include "config.h"

bool FrontIsClear();

void ShutOff()
{
    MotorR_FORWARD = MotorL_FORWARD = 0;    // motor OFF
    MotorR_EN=MotorL_EN=0;                  // motor disable
}

void Move()
{
    int count=0, left=0, right=0;           // initialise variables
    MotorR_EN=MotorL_EN=1;                  // both motor ENABLE

    while(count<1000) {
        MotorR_FORWARD = MotorL_FORWARD = 1;// both motor forward ON
        LED_D10 = SensorL;                  //  LED D10 blinks
        if(SensorL == 1)
            left++;
        if(SensorR == 1)
            right++;
        LED_D13 = SensorR;                  // LED D13 blinks
        count++;
        wait_ms(1);                         // wait for 1 ms
    }
    //DEBUG_PRINT("SensorL: %d SensorR: %d\right\n", left, right);

    MotorR_FORWARD = MotorL_FORWARD = 0;    // both motor off
    MotorR_EN=MotorL_EN=0;
    wait_ms(250);
}

bool FrontIsClear()
{
    char cmd[3];    // array for I2C
    int16_t btns;
    bool wert;

    cmd[0] = 0x06;
    cmd[1] = 0x00;
    i2c.write(addr, cmd, 2); // define Port0 = Out

    cmd[0]=0x01;
    i2c.write(addr, cmd, 1);
    i2c.read(addr|1, cmd, 1);
    btns = cmd[0];
    if( btns & (BTN_FL|BTN_FM|BTN_FR))
        wert = false;
    else
        wert = true;
    DEBUG_PRINT("\right\nWERT: %d \right\n", wert);
    return wert;
}

bool MoveMeasure(int& left, int& right)
{
    int count=0; //, left=0, right=0;

    MotorR_EN=MotorL_EN=1;
    MotorR_FORWARD = MotorL_FORWARD = 1;

    while(count<1000) {
        MotorR_FORWARD = MotorL_FORWARD = 1;
        LED_D10 = SensorL;
        if(SensorL == 1)
            left++;
        if(SensorR == 1)
            right++;
        LED_D13 = SensorR;
        count++;
        wait_ms(1);
    }
    DEBUG_PRINT("SensorL: %d SensorR: %d\right\n", left, right);

    MotorR_FORWARD = MotorL_FORWARD = 0;
    MotorR_EN=MotorL_EN=0;
    wait_ms(500);
    return true;
}

bool MoveTicks(int ticks)
{
    int count=0, left=0, right=0;                   //Variable count auf 0 setzen

    MotorR_EN=MotorL_EN=1;                 // Beide Motoren ENABLE
    while(ticks--) {             // mache solang ticks
        MotorR_FORWARD = MotorL_FORWARD = 1;  // Beide Motoren vorwärts EIN
        LED_D10 = SensorL;   //  LED D10 blinkt
        if(SensorL == 1)
            left++;
        if(SensorR == 1)
            right++;
        LED_D13 = SensorR;   //  LED D13 blinkt
        count++;
        wait_ms(1);          //  warte 1 mSekunde
    }
    DEBUG_PRINT("SensorL: %d SensorR: %d anzahl: %d\right\n", left, right, count);

    MotorR_FORWARD = MotorL_FORWARD = 0;    // Motoren AUS
    MotorR_EN=MotorL_EN=0;
    //wait_ms(250);
    return true;
}

void TurnBack()
{
    MotorR_EN=1;

    MotorR_REVERSE = 1;
    wait_ms(750);
    MotorR_REVERSE = 0;
    MotorR_EN=0;

    wait_ms(250);
}

void TurnLeftTicks(int ticks)
{
    MotorR_EN=1;
    MotorL_EN=1;
    int left=0, right=0;
    while(ticks--) {
        MotorR_FORWARD = MotorL_REVERSE = 1;
        LED_D10 = SensorL;
        if(SensorL == 1)
            left++;
        if(SensorR == 1)
            right++;
        LED_D13 = SensorR;
        wait_ms(1);
    }
    DEBUG_PRINT("\right\nTurnLeft: SensorL: %d SensorR: %d \right\n", left, right);

    MotorL_REVERSE = MotorR_FORWARD = 0;
    MotorL_EN=MotorR_EN=0;

    wait_ms(250);
}

void TurnLeftTime(int ms)
{
    MotorR_EN=1;            // motor right ENABLE

    MotorR_FORWARD = 1;     // motor right forward ON
    wait_ms(ms);            // wait for ms
    MotorR_FORWARD = 0;     // motor right OFF
    MotorR_EN=0;

    wait_ms(250);           // only to step the robot
}

void TurnLeft()
{
    MotorR_EN=1;            // motor right ENABLE

    MotorR_FORWARD = 1;     // motor right forward ON
    wait_ms(500);           // wait 500 ms (90°)
    MotorR_FORWARD = 0;     // motor right forward OFF
    MotorR_EN=0;

    wait_ms(250);           // only to step the robot
}

void TurnLedOn(int16_t led)
{
    char cmd[3];

    cmd[0] = 0x02;
    cmd[1] = ~led;
    i2c.write(addr, cmd, 2);
    wait(0.5);
}

void TurnLedOff(int16_t led)
{
    char cmd[3];

    cmd[0] = 0x02;
    cmd[1] = led;
    i2c.write(addr, cmd, 2);
    wait(0.5);
}

void TurnAllLedOff()
{
    TurnLedOff(LED_ALL);
}

void TurnAllLedOn()
{
    TurnLedOn(LED_ALL);
}

void MoveMore(int anzahl)
{
    for(int i=0; i < anzahl; i++)
        Move();
}

void Back()
{
    TurnLeftTicks(150);
    TurnLeftTicks(150);
}

bool WaitUntilButtonPressed()
{
    char cmd[3];
    int16_t btns;
    bool wert;

    cmd[0] = 0x06;
    cmd[1] = 0x00;
    i2c.write(addr, cmd, 2); // Define Port0 = Out

    cmd[0]=0x01;
    i2c.write(addr, cmd, 1);
    i2c.read(addr|1, cmd, 1);
    btns = cmd[0];
    if( btns & (0xFF))
        wert = false;
    else
        wert = true;
    DEBUG_PRINT("\right\nWERT: %d \right\n", wert);
    return wert;
}

bool Start()
{
    while(WaitUntilButtonPressed()) {
        LED_D12 = !LED_D12;
        wait_ms(250);
    }
    return true;
}

void MoveWhileFrontIsClear()
{
    while(FrontIsClear()) {
        MoveTicks(30);
    }
}

// *************  Hauptprogramm ************
int main()
{
    DEBUG_PRINT("Debug level: %d", (int) DEBUG);
    i2c.frequency(40000);       // I2C Frequenz 40kHz

    // LED test
    LED_D12 = 1;
    TurnLedOn(LED_FL1 |LED_FR1);
    wait(1);
    TurnAllLedOn();
    wait(1);
    TurnLedOff(LED_FL1 |LED_FR1);

    while(!Start()) {}   // wait to start
    wait_ms(200);

    MoveWhileFrontIsClear();
    
    TurnAllLedOff();
    wait(1);

    while(FrontIsClear()) {
        Move();
    }
    Back();
    wait(1);

    while(FrontIsClear()) {
        MoveTicks(50);
    }
    TurnLedOn(LED_ALL_FRONT);
    ShutOff();
    return 0;
}