File content as of revision 3:c7e5419fa980:

/***********************************
name:   Karel
author: PE HTL BULME
email:  pe@bulme.at
description:
    Funktionen von Karel The Robot 
    
      
***********************************/
#include "mbed.h"

const int BTN_FLL = 0x80;
const int BTN_FL  = 0x04;
const int BTN_FM  = 0x01;
const int BTN_FR  = 0x08;
const int BTN_FRR = 0x40;
const int BTN_BL = 0x10;
const int BTN_BM = 0x02;
const int BTN_BR = 0x20;
bool frontIsClear();
void shutOff()
{
 /*   DigitalOut MotorL_EN(p34);
    DigitalOut MotorL_FORWARD(P1_1);
    DigitalOut MotorR_EN(p36);
    DigitalOut MotorR_FORWARD(P1_3);
*/    PwmOut mg1(p34);             //PWM Ausgang zum Motor
    PwmOut mg2(p36);
    mg1=mg2=0.0;

 //    MotorR_FORWARD = MotorL_FORWARD = 0;    // Motoren AUS
//    MotorR_EN=MotorL_EN=0;   
}
bool move()
{
    DigitalOut MotorL_EN(p34);
    DigitalOut MotorL_FORWARD(P1_0);
    //DigitalOut MotorL_REVERSE(P1_1);

    DigitalOut MotorR_EN(p36);
    DigitalOut MotorR_FORWARD(P1_4);
    //DigitalOut MotorR_REVERSE(P1_3);
    PwmOut mg1(p34);             //PWM Ausgang zum Motor
    PwmOut mg2(p36);
    if(!frontIsClear()) {
        mg1=mg2=0.0;
        return false;
    } else {
        mg1=mg2=1.0;
        MotorR_EN=MotorL_EN=1;                 // Beide Motoren ENABLE

        MotorR_FORWARD = MotorL_FORWARD = 1;   // Beide Motoren vorwärts EIN
        wait_ms(250);                           // warte 0,25 Sekunde
        MotorR_FORWARD = MotorL_FORWARD = 0;    // Motoren AUS
        MotorR_EN=MotorL_EN=0;
        //mg1=mg2=0.0;
        wait_ms(250);
        return true;
    }
}

void moveSpeed(float speedy)
{  
    DigitalOut MotorL_EN(p34);
    DigitalOut MotorL_FORWARD(P1_0);
    //DigitalOut MotorL_REVERSE(P1_1);

    DigitalOut MotorR_EN(p36);
    DigitalOut MotorR_FORWARD(P1_4);
    //DigitalOut MotorR_REVERSE(P1_3);
    PwmOut mg1(p34);             //PWM Ausgang zum Motor
    PwmOut mg2(p36);
    mg1=mg2=speedy;
    
    MotorR_EN=MotorL_EN=1;                 // Beide Motoren ENABLE
    MotorR_FORWARD = MotorL_FORWARD = 1;   // Beide Motoren vorwärts EIN
    wait_ms(2000);                           // warte 0,25 Sekunde
    MotorR_FORWARD = MotorL_FORWARD = 0;    // Motoren AUS
    MotorR_EN=MotorL_EN=0;                 // Beide Motoren ENABLE
    wait_ms(500);            
}
void move(int anzahl)
{
  
    for(int i=0; i < anzahl; i++)
        move();
    
}
void turnBack()
{
//    DigitalOut MotorL_EN(p34);
//    DigitalOut MotorL_FORWARD(P1_1);
//    DigitalOut MotorL_REVERSE(P1_0);

    DigitalOut MotorR_EN(p36);
//    DigitalOut MotorR_FORWARD(P1_4);
    DigitalOut MotorR_REVERSE(P1_3);
//  MotorL_EN=1;
    PwmOut mg2(p36);
    mg2=1.0;
    MotorR_EN=1;      // rechten Motor ENABLE

    MotorR_REVERSE = 1;  // rechter Motor vorwärts EIN 
    wait_ms(750);        // warte (90°)
    MotorR_REVERSE = 0;  // Motoren AUS
    MotorR_EN=0;
    
    wait_ms(250);        // warte (90°)
}

void turnLeft()
{
//    DigitalOut MotorL_EN(p34);
//    DigitalOut MotorL_FORWARD(P1_1);
//    DigitalOut MotorL_REVERSE(P1_0);

    DigitalOut MotorR_EN(p36);
    DigitalOut MotorR_FORWARD(P1_4);
//    DigitalOut MotorR_REVERSE(P1_3);
//  MotorL_EN=1;
    PwmOut mg2(p36);
    mg2=1.0;
    MotorR_EN=1;      // rechten Motor ENABLE

    MotorR_FORWARD = 1;  // rechter Motor vorwärts EIN 
    wait_ms(500);        // warte (90°)
    MotorR_FORWARD = 0;  // Motoren AUS
    MotorR_EN=0;
    
    wait_ms(250);        // warte (90°)
}
bool frontIsClear()
{
    I2C i2c(p28,p27);
    const int addr = 0x40; // I2C-Adresse PCA9555
    char cmd[3]; 
    int16_t btns;
    i2c.frequency(40000);  // I2C Frequenz 40kHz

    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))
        return false;
    else
        return true;
}

/*bool IsAnyFrontButton()
            { return btns & (BTN_FL|BTN_FM|BTN_FR); }
*/
// *************  Hauptprogramm ************
int main()
{
    //move();
    //turnLeft();
/*    DigitalOut LED_D10(P1_8);    // LED D10 und D13 definieren
    DigitalOut LED_D11(P1_9);    // LED D10 und D13 definieren
    DigitalOut LED_D12(P1_10);    // LED D10 und D13 definieren
    DigitalOut LED_D13(P1_11);   //
*/
    while(frontIsClear()) {}

    move();
    turnLeft();
    if(move())
        turnBack();
    if(move())
        turnBack();
    if(move())
        turnBack();
    wait(1);
/*    moveSpeed(0.25);
    LED_D10 = 1;
    moveSpeed(0.3);
    LED_D11 = 1;
    moveSpeed(0.4);
    LED_D12 = 1;
    moveSpeed(0.5);
    LED_D13 = 1;
    moveSpeed(0.6);
    LED_D10 = LED_D11 = LED_D12 = LED_D13 = 0;
    moveSpeed(0.7);
    LED_D10 = 1;
    moveSpeed(0.8);
    LED_D11 = 1;
    moveSpeed(0.9);
    LED_D12 = 1;
    moveSpeed(1.0);
    LED_D10 = LED_D11 = LED_D12 = LED_D13 = 1;
    PwmOut mg1(p34);             //PWM Ausgang zum Motor
    PwmOut mg2(p36);
    mg1=mg2=0.0;
    wait(2);
*/    
    move(5);
    turnLeft();
    move();
    turnLeft();
    shutOff();
    //turnLeft();
    //move(5);

    //wait(2);
    return 0;
}