File content as of revision 0:15a8480061e8:

#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, setX, setY, fine, take, test};
    int time = 0;                       //Time keeps track of time. [time] = ms
    
    PID_Control pid;
    pid.setPIDValues( 0.001f, 0.001f, 0.00015f, 0.3f, -0.3f, 1000);
    pc.baud( 115200 );
    
    int state = search;
    
    sam.stop();
    
    /*
    while( 1 ){
        printf("\n\rX: %d,\t Y: %d", pixy.getX(), pixy.getY());
        wait(1.0f);
    }
    */
    
    while( 1 ){
        switch( state ){
            case test:
            int x = pixy.getX(), y = pixy.getY();
                if( !mybutton ) printf("\n\rX:%d\tY:%d", x, y);
                break;
                
            case search:
                if( sam.search(&time) ) state = found;
                break;
                
            case setX:{
                static int messungen[20], i = 0;
                float e = 132.5f - pixy.getX();
                float diff = pid.calc( e, 0.001f );
                
                sam.setLeft(0.5f - diff);
                sam.setRight(0.5f - diff);
                break;
            }
            case setY:{
                static int messungen[20], i = 0;
                float e = 121.5f - pixy.getY();
                float diff = pid.calc( e, 0.001f );
                
                sam.setLeft(0.5f + diff);
                sam.setRight(0.5f - diff);
                break;
            }
            
            case take:{
                //sam.leds[1] = 1;
                sam.stop();
                while (1) wait(0.1f);
                static int down = 0, closed = 0, up = 0;
                if( down || sam.Arm.backToDown() )         down = 1;
                else if( closed || sam.Greifer.take() )    closed = 1;
                else if( up || sam.Arm.downToBack() )      up = 1;
                else if( sam.Greifer.leave() ){
                    state = search;
                    down = 0;
                    closed = 0;
                    up = 0;
                }
                break;
            }
        }
        time++;
        wait(0.001f);
    }
        
    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;
}
/* */