File content as of revision 5:acb938f45b9c:

#include "mbed.h"
#include "Robot.h"
#include "Declarations.h"
#include "Pixy.h"
#include "PID_Control.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);

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

//Button:
DigitalIn mybutton(USER_BUTTON);

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


Robot sam( &left, &right, &powerSignal, leds, &FarbVoltage, &frontS, &leftS, &rightS, &servoArm, &servoGreifer, &pixy ); //Robot-Constructor

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

void competition(){
    initializeDistanceSensors();            //Initialises IR-Sensors
    enum states {search = 0, setPos, take}; //3-states machine
    int time = 0;                           //Time keeps track of time

    PID_Control pid;
    pid.setPIDValues( 0.001f, 0.001f, 0.00015f, 0.15f, -0.15f, 1000);
    
    
    
    int state = search;
    sam.Arm.init(&servoArm);
    while(!sam.Arm.collectToBack())wait(0.075f); 
    while(!sam.Greifer.leave())wait(0.001f);


    while(1){
        if(time>TIMEOUT)return;
        
        switch(state){
    
        case search:{
                if(!((pixy.getX()>50 && pixy.getX()<250)&&(pixy.getY()>50 && pixy.getY()<300))){
                   sam.search(&time);     
                }
                else{
                    state = setPos;
                    time = 0;
                }
                break;
            }
            
        case setPos: {
            time++;
            sam.leds[5] = 1;
            static int i = 0;
            if(!((pixy.getX()>50 && pixy.getX()<250)&&(pixy.getY()>50 && pixy.getY()<300))){
                time=0;
                state=search;
                i=0;
            }
            float eX = 131.0f - pixy.getX();
            float diffX = pid.calc( eX, 0.006f );
            
            float aX = 0.04;             //minimum diff X
            float aY = 0.04;             //minimum diff Y
            if(diffX>0) {          
                if(diffX<aX)diffX=aX;
            }
            else if(diffX>-aX)diffX=-aX;
            
            //Set the X position
            if(!(pixy.getX()>130 && pixy.getX()<133)){
            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;
            
            //Set the Y position
            if(!(pixy.getY()>119 && pixy.getY()<121)){
            sam.setLeft(0.5f + diffY);
            sam.setRight(0.5f - diffY);
            }
            i++;
            if((pixy.getX()>130 && pixy.getX()<132)&&(pixy.getY()>120 && pixy.getY()<122)||(!(i%1000))) {
                state = take;
                i = 0;
                sam.stop();
                sam.leds[5] = 0;
            }
            break;
        }
    
        case take:{
                if(time>TIMEOUT)return;
                enum takeStates {down=0, take, up, leave}; //4-states machine
                static int tState=down;
    
                sam.leds[1] = 1;
                sam.stop();
                
                switch(tState){
                    case down:
                        if(sam.Arm.backToDown()){
                             sam.Greifer.leave();
                             tState=take;
                        }
                        else tState=down;
                        break;
                    case take:
                        if(sam.Greifer.take()) tState=up;
                        else tState=take;
                        break;
                    case up:
                        if(sam.Arm.downToBack()) 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); //Cycle-period
    }
}
int main(){
    int start = 0;
    while( 1 ){
        if( !mybutton ){
             start++;
             sam.Arm.init(&servoArm);                //Arm initialisation
             sam.Greifer.init(&servoGreifer);        //Greifer initialisation
        }      
        if( start ){
             competition();
             sam.stop();
        }
         wait(0.1f);
    }
}