File content as of revision 0:d267b248eff4:

#include "mbed.h"
#include "Robot.h"

#include <cstdlib>

#define NEAR 0.1f   //Used for distance Sensors. If they're to near to a wall -> turn
#define LEFT        //Arrayindex of the left Sensor
#define FWD         //Arrayindex of the forward Sensor
#define RIGHT       //Arrayindex of the right Sensor


//Robot related:
PwmOut          left(PA_8);
PwmOut          right(PA_3);

DigitalOut      powerSignal(PB_2);
DigitalIn       errorSignal(PB_14);
DigitalIn       overtemperatur(PB_15);

Robot bot( &left, &right, &powerSignal );

//DistanceSensors related:
Serial pc(SERIAL_TX, SERIAL_RX);

AnalogIn sensorVoltage(PB_1);
DigitalOut enable(PC_1);
DigitalOut bit0(PH_1);
DigitalOut bit1(PC_2);
DigitalOut bit2(PC_3);
//Robot::DistanceSensors sensors[6];

//LED related:
DigitalOut leds[] = { PC_8, PC_6, PB_12, PA_7, PC_0, PC_9 };

void initialiseDistanceSensors(){
    for( int ii = 0; ii<6; ++ii) {
        bot.sensors[ii].init(&sensorVoltage, &bit0, &bit1, &bit2, ii);

        enable = 1;
    }
}

void fahralgorithmus(){
    
    int rando = 0; //Will be used for rdandom actions.

    while( 1 ) {    //Remember to change the indices. Also make into a seperate function

        if ( bot.sensors[0] < NEAR ) { //Left Sensor
            while ( bot.sensors[0] < NEAR ) {
                bot.turnRight();
            }
        }

        else if ( bot.sensors[1] < NEAR ) { //Front Sensor
            rando = rand() % 2;
            while ( bot.sensors[1] < NEAR ) {
                bot.turnAround(rando);
            }
        }

        else if ( bot.sensors[2] < NEAR ) { //Right Sensor
            while ( bot.sensors[2] < NEAR ) {
                bot.turnLeft();
            }
        }

        else {
            bot.drive();
        }

    }
}

/* * /


int main(){
    while( 1 ){
        bot.drive();
    }
    return 0;
}
/* */


/* */
int main()
{
    initialiseDistanceSensors();
    
    while( 1 ){
        for(int i=0; i<6; i++){
            if(bot.sensors[i] > 0.05f){
                leds[i] = 1;
            }
            else{
                leds[i] = 0;
            }
        }
    }
    
    return 0;
}
/*  */