File content as of revision 16:f157e5ccd7d3:

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

//Leiste:
Servo servoLeiste(PB_6);

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

Robot sam( &left, &right, &powerSignal, leds, &FarbVoltage, &frontS, &leftS, &rightS, &servoArm, &servoGreifer, &servoLeiste ); //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;
    int color;
    
    enum states { search = 0, LeisteDown, push, backOff, forward, downward, down, upward, colorS, readyDrop, backward, LeisteUp };

    int state = search;
    
    while( 1 ) {
        
        if ( timer > TIMEOUT ) {
            NVIC_SystemReset();         //Resets Sam.
        }
        
        switch( state ) {
            case search:
                if( sam.search(&timer) ){
                    //sam.Greifer.nullPos();
                    state = LeisteDown;
                    timer = 0;
                }
                break;
       
            case LeisteDown:
                if( sam.Leiste.upToDown() ){
                    //sam.Greifer.leave();
                    state = push;
                    timer = 0;
                }
                break;
                
            case push:{
                static int i = 0;
                if( i > 5 ){
                    sam.stop();
                    i = 0;
                    state = backOff;
                    timer = 0;
                }
                else{
                    sam.driveSlowly();
                    i++;
                }
                break;
            }
            
            case backOff:{
                static int i = 0;
                if( i > 5 ){
                    sam.stop();
                    i = 0;
                    state = forward;
                    timer = 0;
                }
                else{
                    sam.driveBackSlowly();
                    i++;
                }
                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 = colorS;
                    timer = 0;
                }
                break;
            
            case colorS:
                led = 1;
                color = sam.FarbVoltage.read();
                
                if( color == -1 ){
                    //Do nothing
                }
                
                
                else if( color == 0 || color == GREEN ){
                    state = backward;
                    led = 0;
                    timer = 0;
                }
                
                else if( color == RED ){
                    state = readyDrop;
                    led = 0;
                    timer = 0;
                }
                
                else{
                    //Shit...
                }
                break;
             
            case readyDrop:
                if( sam.Greifer.leave() ){
                    if( color == GREEN || color == 0 ){
                        state = LeisteUp;
                    }
                    else{
                        state = backward;
                    }
                    timer = 0;
                }
                
                break;
                
            case backward:
                if( sam.Arm.collectToBack() ){
                    //sam.Greifer.nullPos();
                    state = LeisteUp;
                    timer = 0;
                    if( color == GREEN || color == 0 ){
                        state = readyDrop;
                        sam.Greifer.leave();
                    }
                    else{
                        state = LeisteUp;
                    }
                }
                break;
                
            case LeisteUp:
                if( sam.Leiste.downToUp() ){
                    state = search;
                    timer = 0;
                }
                break;
        }
        
    /*    
            sam.leds[1] = sam.sensors[RIGHT] < NEAR;
            sam.leds[5] = sam.sensors[LEFT] < NEAR;
            sam.leds[4] = sam.sensors[FWD] < NEAR;
      */  
        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();
    
    //DigitalOut userLed(LD2);

    while ( 1 ){
        sam.sensors[FWD_L] < NEAR ?       sam.leds[4] = 1         : sam.leds[4] = 0;
        sam.sensors[RIGHT_L] < NEAR ?     sam.leds[RIGHT_L] = 1   : sam.leds[RIGHT_L] = 0;
        sam.sensors[LEFT_L] < NEAR ?      sam.leds[LEFT_L] = 1    : sam.leds[LEFT_L] = 0;
        //sam.sensors[FWD_L] < NEAR ?     sam.leds[2] = 1 : sam.leds[4] = 0;
        //sam.sensors[RIGHT_L] < NEAR ?   sam.leds[3] = 1 : sam.leds[4] = 0;
        //sam.sensors[LEFT_L] < NEAR ?    userLED     = 1 : sam.leds[4] = 0;
        
        wait (0.2f);
    }

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

*/