File content as of revision 2:c8c965d48f8d:

#include "mbed.h"
#include "Robot.h"
#include "Declarations.h"
#include "Ultraschall.h"
#include "Pixy.h"
#include "PID_Control.h"
#include "LowpassFilter.h"
#include "EncoderCounter.h"

#include <cstdlib>

//Cam:
Serial cam(PA_9, PA_10);
Pixy pixy(cam);

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


//Ultraschall
Ultraschall usensor(D6,D5);

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

//Button:
DigitalIn mybutton(USER_BUTTON);

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


Robot sam( &left, &right, &powerSignal, leds, &FarbVoltage, &frontS, &leftS, &rightS, &servoArm, &servoGreifer, &servoLeiste, &usensor, &pixy ); //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.
    enum states {search = 0, setPos, take};
    int time = 0;                       //Time keeps track of time. [time] = ms

    PID_Control pid;
    pid.setPIDValues( 0.001f, 0.001f, 0.00015f, 0.15f, -0.15f, 1000);
    // pc.baud( 115200 );

    int state = search;
    while(!sam.Arm.collectToBack())wait(0.075);
    while(!sam.Greifer.leave())wait(0.001);


    while( 1 ) {
        
    switch(state){
    
    case search: {
            if(!((pixy.getX()>50 && pixy.getX()<300)&&(pixy.getY()>50 && pixy.getY()<300))){
               sam.search(&time);     
            }
            else {
                state = setPos;
                }
           
            break;
        }
        
    case setPos: {
        sam.leds[5] = 1;
        static int i = 0;
        float eX = 133.0f - pixy.getX();
        float diffX = pid.calc( eX, 0.006f );
        
        float aX = 0.03;             //minimum diff 
        float aY = 0.05;
        if(diffX>0) {
            if(diffX<aX)diffX=aX;
        }
        else if(diffX>-aX)diffX=-aX;
        
        if(!(pixy.getX()>128 && pixy.getX()<138)){
        sam.setLeft(0.5f - diffX);
        sam.setRight(0.5f - diffX);
        }
       
        wait(0.001f);
        
        float eY = 121.0f - pixy.getY();
        float diffY = pid.calc( eY, 0.006f );
        if(diffY>0) {
            if(diffY<aY)diffY=aY;
        }
        else if(diffY>-aY) diffY=-aY;
        if(!(pixy.getY()>115 && pixy.getY()<125)){
        sam.setLeft(0.5f + diffY);
        sam.setRight(0.5f - diffY);
        }
        i++;
        if((pixy.getX()>132 && pixy.getX()<134)&&(pixy.getY()>119 && pixy.getY()<122)||!(i%1000)) {
            state = take;
            i = 0;
            sam.stop();
            sam.leds[5] = 0;
        }
        break;
    }

    case take: {
            enum takeStates {down=0, take, up, leave};
            static int tState=down;

            sam.leds[1] = 1;
            sam.stop();
            //tState=leave;

            switch(tState) {
                case down:
                    if(sam.Arm.backToDown()) tState=take;
                    else tState=down;
                    break;
                case take:
                    if(sam.Greifer.take()) tState=up;
                    else tState=take;
                    break;
                case up:
                    if(sam.Arm.downToBack()) {wait(0.05);tState=leave;}
                    else tState=up;
                    break;
                case leave:
                    if(sam.Greifer.leave()) {state=search; tState=down;}
                    else tState=leave;
                    break;
            }


            sam.leds[1] = 0;
            break;
        }
    }
    time++;
    wait(0.005f);
}

return 0;
}

/* * /
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, turn, push, backOff, forward, downward, down, upward, colorS, readyDrop, backward, LeisteUp };

    int state = search;

                static float messung = 0;

    while( 1 ){
        printf("\n\r%f", sam.see(FWD_L));

        wait(1.0f);

    }

    while( 1 ) {

        if ( timer > TIMEOUT ) {
            NVIC_SystemReset();         //Resets Sam.
        }

        //if( timer == 0 )
        //printf("\n\rLEFT: %.3f,\tFWD: %.3f,\tRIGHT: %.3f", sam.sensors[LEFT].read(), sam.sensors[FWD].read(), sam.sensors[RIGHT].read());


        //printf("\n\rcurrent main state: %d", state);

        sam.sensors[FWD_L].read() < NEAR ? sam.leds[1] = 1 : sam.leds[1] = 0;
        switch( state ) {
            case search:
                if( sam.search(&timer) ){
                    //sam.Greifer.nullPos();
                    state = LeisteDown;
                    timer = 0;
                }
                break;

            case LeisteDown:
                int count = 0;
                if( sam.Leiste.upToDown() ){
                    //sam.Greifer.leave();
                    state = turn;
                    timer = 0;
                }
                break;

            case turn:
                static int i = 0;
                if( i > 7 ){
                    sam.stop();
                    state = push;
                    i = 0;
                }
                else{
                    i++;
                    sam.turnRight();
                }
                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 > 1 ){
                    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;
        }
        timer++;
        wait(0.1f);
    }

    return 0;
}
/* */