File content as of revision 9:8e3392380915:

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

//bool Start(int speed);
bool Start(float speed);
void ShutOff();
void Move();
bool FrontIsClear();
bool FrontIsClearU(); // with ultra sonic sensor
bool MoveMeasure(int time, int& left, int& right);
bool MoveTicks(int ticks);
void TurnBack();
void TurnLeftTicks(int ticks);
void TurnLeftTime(int ms);
void TurnLeft();
void TurnLedOn(int16_t led);
void TurnLedOff(int16_t led);
void MoveMore(int anzahl);
void Back();
bool WaitUntilButtonPressed();
int BottomIsBlack();    // with line sensor

// Eigene Funktionsdefinitionen hier

void LedAround()
{
    int i=10;
    int led = 0;

    led = LED_FL1;  // 0x01

    while(i-- > 0) {
        TurnLedOn(led);
        wait(1);
        led *= 2;
        if(led > 0x80)
            led = LED_FL1;  // 0x01
    }
}

// *************  Hauptprogramm ************
int main()
{
    Start(0.3);     // speed between 0.2 (low) and 1.0 (normal)
    //Move();
    while(!BottomIsBlack())
    {
        MoveTicks(40);
    }
    TurnLeft();
    while(FrontIsClearU())
    {
        MoveTicks(40);
    }

    TurnLeftTicks(200);    // Anzahl der Motorsensorwerte
    Move();
    ShutOff();
    return 0;
}

// Line sensor at bottom of Bertl
int BottomIsBlack()
{
    int detect;
    
    detect = linesensor;
    wait_ms(5);
    return detect;
}

bool Start(float speed)
{
    pc.baud(9600);
    pc.printf("Hello");

    DEBUG_PRINT("Debug level: %d", (int) DEBUG);
    i2c.frequency(40000);       // I2C Frequenz 40kHz
    mg1 = mg2 = speed;
//    while(WaitUntilButtonPressed()) {    }
    return true;
}

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;
}
// with ultra sonic sensor 
bool FrontIsClearU()
{
    int dist = 0;
    usensor.start();
    wait_ms(10);
    dist=usensor.get_dist_cm();
    if(dist < 5)
        return false;
    else
        return true;
    DEBUG_PRINT("Distance: %d", dist);
}
/*
use in main():
    int left=0, right=0;

    MoveMeasure(1000, left, right);
    pc.printf("\r\nleft= %d  right= %d\r\n", left, right);
*/
bool MoveMeasure(int time, int& left, int& right)
{
    int count=0; //, left=0, right=0;

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

    while(count<time) {
        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 = MotorL_REVERSE = 1;
    //MotorR_FORWARD = 1;     // motor right forward ON
    wait_ms(500);           // wait 500 ms (90°)
    //MotorR_FORWARD = 0;     // motor right forward OFF
    //MotorR_EN=0;
    MotorL_REVERSE = MotorR_FORWARD = 0;
    MotorL_EN=MotorR_EN=0;
    wait_ms(250);           // only to step the robot
}

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;
}

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 ManuelDistancMeasure()
{
    int dist = 0;
    unsigned char count=0;

    while(count < 20) {
        usensor.start();
        wait_ms(500);
        dist=usensor.get_dist_cm();
        pc.printf("\r\nCount =%d",count);
        pc.printf("Distance: %d",dist );
        count++;

    }
}

/*
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);
}

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

void LedTest()
{
    TurnLedOn(LED_FL1 |LED_FR1);
    wait(1);
    TurnAllLedOn();
    wait(1);
    TurnLedOff(LED_FL1 |LED_FR1);
}
*/