File content as of revision 4:e360384c268b:

#include "mbed.h"
#define VRIJEME_KRETANJA 0.1
#define STANI 0
#define CW 0.0014
#define CCW 0.0015


Serial pc(SERIAL_TX, SERIAL_RX);

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


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(D4);
PwmOut pwmg(D9);

Timeout cekanje;
Timer tKrug, tPozicija, tOstatak;
int logikaVrmena, t2=0, t3=0;


void ocitanje()
{

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

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:
            gore();
            pc.printf("\n\nNALAZIMO SE U POZICIJI %d\n", pozicija);
            break;
        case 2:
            desno();
            pc.printf("\n\nNALAZIMO SE U POZICIJI %d\n", pozicija);
            break;
        case 3:
            dolje();
            pc.printf("\n\nNALAZIMO SE U POZICIJI %d\n", pozicija);
            break;
        case 4:
            lijevo();
            pc.printf("\n\nNALAZIMO SE U POZICIJI %d\n", pozicija);
            break;
        case 5:
            desno();
            gore();
            pc.printf("\n\nNALAZIMO SE U POZICIJI %d\n", pozicija);
            break;
        case 6:
            desno();
            dolje();
            pc.printf("\n\nNALAZIMO SE U POZICIJI %d\n", pozicija);
            break;
        case 7:
            lijevo();
            dolje();
            pc.printf("\n\nNALAZIMO SE U POZICIJI %d\n", pozicija);
            break;
        case 8:
            lijevo();
            gore();
            pc.printf("\n\nNALAZIMO SE U POZICIJI %d\n", pozicija);
            break;
        case 9:
            stani();
            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();


    }
}