File content as of revision 12:c0bcb95885dd:

#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       errorMotor(PB_14);
DigitalIn       overtemperatur(PB_15);

//Arm:
ServoArm servoArm(PA_6);

//Greifer:
Servo servoGreifer(PC_7);

//Farbsensor:
AnalogIn FarbVoltage(A0);
DigitalOut led(D2);

Robot sam( &left, &right, &powerSignal, leds, &FarbVoltage, &frontS, &leftS, &rightS, &servoArm, &servoGreifer ); //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
    //int done = 0;

    enum states { search = 0, forward, downward, down, upward, color, backwardDrop, readyDrop, backward };

    int state = downward;

    while( 1 ) {
        
        if ( timer > TIMEOUT ) {
            NVIC_SystemReset();         //Resets Sam.
        }
        
        switch( state ) {
            case search:
                if( sam.search(&timer) ){
                  state = forward;
                  timer = 0;
                }
                
                break;
            
            case forward:
                if( sam.Arm.backToCollect() ){
                    state = downward;
                    timer = 0;
                }
                
                break;

            case downward:
                if( sam.Arm.collectToDown() ){
                    state = down;
                    timer = 0;
                }
                
                break;

            case down:
                if( sam.Greifer.take() ) {
                    state = upward;
                    timer = 0;
                }
                
                break;
            
            case upward:
                if( sam.Arm.downToCollect() ){
                    state = color;
                    timer = 0;
                }
                
                break;
            
            case color: {
                led = 1;
                int color = sam.FarbVoltage.read();
                
                if( color == -1 ){
                    //Do nothing
                }
                
                
                else if( color == 0 || color == GREEN ){
                    state = backwardDrop;
                    led = 0;
                    timer = 0;
                }
                
                else if( color == RED ){
                    state = readyDrop;
                    led = 0;
                    timer = 0;
                }
                
                else{
                    //Shit...
                }
                break;
            }
            
            case backwardDrop:
                if( sam.Arm.collectToBack() ){
                    state = readyDrop;
                    timer = 0;
                }
                
                break;
                
            case readyDrop:
                if( sam.Greifer.leave() ){
                    state = backward;
                    timer = 0;
                }
                
                break;
                
            case backward:
                if( sam.Arm.collectToBack() ){
                    state = downward;
                    timer = 0;
                }
                
                break;
        }
        
        timer++;
        wait(0.1f);
    }
    
    return 0;
}
        
        
        
        /*
        if (sam.search(&counter, &timer)) {

            while(!done) {
                sam.Arm.collectToDown(&done);
                wait(0.1f);
            }
            done = 0;

            sam.Greifer.take();

            while(!done) {
                sam.Arm.downToCollect(&done);
                sam.leds[1] = 1;
                wait(0.1f);
            }
            done = 0;

            sam.Greifer.leave();

            found = 0;
        }
        

        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.
    int start = 0;


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

*/