File content as of revision 6:ba26dd3251b3:

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

#include <cstdlib>

//DistanceSensors related bottom:
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);

//DistanceSensors top:
AnalogIn frontS(A1);
AnalogIn leftS(A2);
AnalogIn rightS(A3);

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

//motor related:
PwmOut          left(PA_8);
PwmOut          right(PA_9);

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

//Arm:
Servo servoArm(PA_6);

//Farbsensor:
AnalogIn FarbVoltage(A0);

Robot sam( &left, &right, &powerSignal, leds, &FarbVoltage, &frontS, &leftS, &rightS, &servoArm); //Implement the Farbsensor into the Robot init function!!

void initializeDistanceSensors(){
    for( int ii = 0; ii<9; ++ii) {
        sam.sensors[ii].init(&sensorVoltage, &frontS, &leftS, &rightS, &bit0, &bit1, &bit2, ii);

    enable = 1;
    }
}

/* * /
int main(){
    initializeDistanceSensors();        //Initialises IR-Sensors.
    int counter = 0;                    //Counts how many times the robot has turned, before driving
    int timer = 0;                      //Is used, to reset the robot. Returns time in [0.1s]
    //int lastFun = -1;                   //Is used, to check if the same action in Robot::search() was made multiple times.
    int found = 0;                      //0:= no block available, 1 := a lego is ready to be picked up
    
    while( 1 ){
        if ( timer > TIMEOUT ){
            NVIC_SystemReset();         //Resets Sam.
        }
        else if (found == 0){
            sam.search(&counter, &timer, &found);
        }
        else{
            //pick up lego
            //found = 0;
            sam.stop(); //Nur zum Testen
        }
        
        wait( 0.1f );
    }
    return 0;
}

/* * /
int main(){
    
    initializeDistanceSensors();
    sam.stop();
    
    while ( 1 ){
        for (int i=0; i<6; i++){
            sam.sensors[i] < NEAR ? sam.leds[i] = 1 : sam.leds[i] = 0;
        }
        
        sam.turnLeftS();
        
        while (sam.sensors[FWD] < NEAR){
            wait(0.05f);
            sam.stop();
        }
        
    }
    return 0;
}
/* */

/* * /
int main(){
    for(float p=0; p<1.0; p += 0.1) {
      //  arm = p;
        wait(0.2);
    }
}
/* */

/* */
int main(){
    sam.stop();
    int done = 0;           //1:= finished process; 0:= not finished
    int fun = 0;            //just to test.
    
    while(1){
        if(fun == 0){
            done = 0;
            sam.Arm.collecttoback(&done);
            done == 0 ? fun = 0 : fun = 1;
        }
        else if(fun == 1){
            done = 0;
            sam.Arm.backtocollect(&done);
            done == 0 ? fun = 1 : fun = 2;
        }
        else if(fun == 2){
            done = 0;
            sam.Arm.collecttodown(&done);
            done == 0 ? fun = 2 : fun = 3;
        }
        else if(fun == 3){
            done = 0;
            sam.Arm.downtocollect(&done);
            done == 0 ? fun = 3 : fun = 0;
        }
        wait(0.1);
    }
}