File content as of revision 1:19dc1ee367a9:

#include "mbed.h"
#include "MPU6050.h"
#include "Pozicija.h"

Serial pc(USBTX, USBRX);
MPU6050 mpu(0x68);
Pozicije poz(mpu);


DigitalOut myled1(LED1);      //Objekti digitalnih izlaza (spojeni na ugradjene LED diode)
DigitalOut myled4(LED4);      //Objekti digitalnih izlaza (spojeni na ugradjene LED diode)

InterruptIn button(p21);      //Interrupt na pin 21

Timer debounce;
Ticker TickTack;


void PrintOut()
{
    myled1=myled4;
    myled4=!myled4; //Blinkanje ledicama

    pc.printf("Trenutne koordinate osi: \n");
    pc.printf("Acc X : %d \r", poz.getAccX() );
    pc.printf("Acc Y : %d \r", poz.getAccY() );
    pc.printf("Acc Z : %d \r", poz.getAccZ() );

    pc.printf("Rotacija X : %d \r", poz.getGyX() );
    pc.printf("Rotacija Y : %d \r", poz.getGyY() );
    pc.printf("Rotacija Z : %d \r", poz.getGyZ() );
}

void Update()
{
    if (debounce.read_ms()>200)   {  //200 ms od zadnjeg interrupta

        if(mpu.testConnection()) {  //Testira konekciju s MPU6050
            
            pc.printf("Konekcija OK\n");
            poz.Update();            
        } else {
            pc.printf("Konekcija !OK\n");
        }
    }

    debounce.reset();               //Reser debounce timera na 0 ms
}

void setup()
{
    mpu.initialize();           //"Budi" mpu i postavlja osnovne parametre

    if(mpu.testConnection()) {  // Testira konekciju
        pc.printf("MPU uspjesno spojen \n");
    } else {
        pc.printf("MPU nije pronaden \n");
    }

    debounce.start();           //Pokreče debounce Timer
    button.rise(Update);       //Na rastuci brid pokreče funkciju "Update"

    TickTack.attach(&PrintOut,2) ;//Svake 2 sekunde pokrece funkciju za ispis pozicija
}


int main()
{
    setup();
}