File content as of revision 4:e360384c268b:

#include "mbed.h"
#define VRIJEME_KRETANJA 0.1
#define STANI 0
#define CW 0.0035
#define CCW 0.006

Serial pc(SERIAL_TX, SERIAL_RX);

AnalogIn in0(A0);
AnalogIn in1(A1);
AnalogIn in2(A2);
AnalogIn in3(A3);
DigitalOut ledica(D4);

float fotootp1=0, fotootp2=0, fotootp3=0, fotootp4=0;
float a12, a13, a14, a21, a23, a24, a31, a32, a34, a41, a42, a43;
int pozicija;

PwmOut pwmd(D5);
PwmOut pwmg(D9);

Timeout cekanje;


void ocitanje(){
       /* fotootp1 = in0.read();
        fotootp2 = in2.read();
        fotootp3 = in2.read();
        fotootp4 = in3.read(); */
        
    fotootp1 = in0.read();
    printf("\n\r%f", fotootp1);
    fotootp2 = in1.read();
    printf("\n\r%f", fotootp2);
    fotootp3 = in2.read();
    printf("\n\r%f", fotootp3);
    fotootp4 = in3.read();
    printf("\n\r%f\n\n", fotootp4);
    wait(2);
    }
    
void zakljucivanje(){
    a12= fotootp1 - fotootp2;
    a13= fotootp1 - fotootp3;
    a14= fotootp1 - fotootp4;
    
    a21 = fotootp2 - fotootp1;
    a23 = fotootp2 - fotootp3;
    a24 = fotootp2 - fotootp4;
    
    a31 = fotootp3 - fotootp1;
    a32 = fotootp3 - fotootp2;
    a34 = fotootp3 - fotootp4;
    
    a41 = fotootp4 - fotootp1;
    a42 = fotootp4 - fotootp2;
    a43 = fotootp4 - fotootp3; 
    
    if (a12 >= 0.2 && a13 >= 0.2 && a14 >= 0.2){
        pozicija = 1;
        }
        
    else if (a21 >= 0.2 && a23 >= 0.2 && a24 >= 0.2){
        pozicija = 2;
        }
        
    else if (a31 >= 0.2 && a32 >= 0.2 && a34 >= 0.2){
        pozicija = 3;
        }
        
    else if (a41 >= 0.2 && a42 >= 0.2 && a43 >= 0.2){
        pozicija = 4;
        }
        
    else if (!(a12 >= 0.2 )&& a13 >= 0.2 && a14 >= 0.2){
        pozicija = 5;
        }
        
    else if (a21 >= 0.2 && !(a23 >= 0.2) && a24 >= 0.2){
        pozicija = 6;
        }
        
    else if (a31 >= 0.2 && a32 >= 0.2 && !(a34 >= 0.2)){
        pozicija = 7;
        }
    
    else if (!(a41 >= 0.2) && a42 >= 0.2 && a43 >= 0.2){
        pozicija = 8;
        }
    else {
        pozicija = 9;
    }
        

}

//OUTPUT     ------------------------------------
void stani(){
    pwmg.period_ms(20);
    while(1){
        pwmg.pulsewidth(STANI);
        break;
        }
    }
    
void gore(){
    pwmg.period_ms(20);
    while(1){
        pwmg.pulsewidth(CCW);
        break;
        }
    }
    
void desno(){
    pwmd.period_ms(20);
    while(1){
        pwmd.pulsewidth(CCW);
        break;
        }
    }
    
void dolje(){
        pwmg.period_ms(20);
    while(1){
        pwmg.pulsewidth(CW);
        break;
        }
    }
    
void lijevo(){
    pwmd.period_ms(20);
    while(1){
        pwmd.pulsewidth(CW);
        break;
        }
    }
    
//OUTPUT -------------------------

void pozicioniranje(){
    switch(pozicija){
        case 1:
            cekanje.attach(&gore, VRIJEME_KRETANJA);
            pc.printf("\n\nNALAZIMO SE U POZICIJI %d\n", pozicija);
            break;
        case 2:
            cekanje.attach(&desno, VRIJEME_KRETANJA);
            pc.printf("\n\nNALAZIMO SE U POZICIJI %d\n", pozicija);
            break;
        case 3:
            cekanje.attach(&dolje, VRIJEME_KRETANJA);
            pc.printf("\n\nNALAZIMO SE U POZICIJI %d\n", pozicija);
            break;
        case 4:
            cekanje.attach(&lijevo, VRIJEME_KRETANJA);
            pc.printf("\n\nNALAZIMO SE U POZICIJI %d\n", pozicija);
            break;          
        case 5:
            cekanje.attach(&desno, VRIJEME_KRETANJA);
            cekanje.attach(&gore, VRIJEME_KRETANJA);
            pc.printf("\n\nNALAZIMO SE U POZICIJI %d\n", pozicija);
            break;
        case 6:
            cekanje.attach(&desno, VRIJEME_KRETANJA);
            cekanje.attach(&dolje, VRIJEME_KRETANJA);
            pc.printf("\n\nNALAZIMO SE U POZICIJI %d\n", pozicija);
            break;
        case 7:
            cekanje.attach(&lijevo, VRIJEME_KRETANJA);
            cekanje.attach(&dolje, VRIJEME_KRETANJA);
            pc.printf("\n\nNALAZIMO SE U POZICIJI %d\n", pozicija);
            break;
        case 8:
            cekanje.attach(&lijevo, VRIJEME_KRETANJA);
            cekanje.attach(&gore, VRIJEME_KRETANJA);
            pc.printf("\n\nNALAZIMO SE U POZICIJI %d\n", pozicija);
            break;
        case 9:
            cekanje.attach(&stani, VRIJEME_KRETANJA);
            pc.printf("\n\nNALAZIMO SE U POZICIJI %d\n", pozicija);
            break;
        default:
            pc.printf("pogresna informacija");
        break;
        }
    }

/*   




void trazenje(){
    while(1){
            //logika za trazenje (spremanje neke vrijednosti) // 3 timera jedan koji broji kolko treba vremena, 
                                                    //jedan se reseta kada nađe najvecu i jedan za odbrojavanje do pozicije
            //logika();
        }
    }
*/