//Treci projekat
//Baudrate 9600 za serijsku komunikaciju
//Freq 20MHz PLL8
#include<p30fxxxx.h>
#include "outcompare.h" //naknadno ubacen h fajl za PWM
#include "timer.h"
#include "timer2.h"
#include "uart.h"

_FOSC(CSW_FSCM_OFF & XT_PLL8);//instruction takt je isti kao i kristal
_FWDT(WDT_OFF);

unsigned int stoperica;
unsigned int br=0;

void __attribute__((__interrupt__)) _T3Interrupt(void)//svakih 10us
{
   	TMR3 =0;
	stoperica++;
    IFS0bits.T3IF = 0;
}

void Delay_us (unsigned int vreme)//funkcija za kasnjenje u milisekundama 0.1ms
{
		stoperica = 0;
		while(stoperica < vreme);
}
void initPins()
{
	//Za drajver L298N in1,in2,in3,in4
    ADPCFGbits.PCFG9=1;//digitalni
    ADPCFGbits.PCFG10=1;
    ADPCFGbits.PCFG11=1;
    ADPCFGbits.PCFG12=1;

    TRISBbits.TRISB9=0;//izlazni
    TRISBbits.TRISB10=0;
    TRISBbits.TRISB11=0;
	TRISBbits.TRISB12=0;
    //------------------------------------------------
	//HC-SR04 Prvi
	TRISDbits.TRISD3=1;//ulazni pin ECHO
	TRISDbits.TRISD9=0;//izlazni pin TRIGGER

	//HC-SR04 Drugi
	TRISDbits.TRISD2=1;//ulazni pin ECHO
	TRISDbits.TRISD8=0;//izlazni pin TRIGGER

	//-------------------------------------------------
	//konfigurisemo kao izlaz za pwm
	TRISDbits.TRISD0=0;
	TRISDbits.TRISD1=0;

	//-------------------------------------------------
	LATBbits.LATB9=0;//RB9,RB10,RB11,RB12 za upravljanje L298
    LATBbits.LATB10=0;
    LATBbits.LATB11=0;
    LATBbits.LATB12=0;

}

int main(void)
{
	initUART1();
	init_T3();
	initPins();
	RS232_putst("Test");
    Delay_us(1000);
	unsigned int dist=0;
    int i;
	while(1)
	{
        //WriteUART1('0');
        for(i=0;i<10000;i++);
		LATDbits.LATD8=1;
		Delay_us(1);
		LATDbits.LATD8=0;
        //WriteUART1('1');
		while(PORTDbits.RD2==0);
        WriteUART1('K');
		while(PORTDbits.RD2==1)
		{
			Delay_us(1);
			br++;
            //WriteUART1('3');
		}
        WriteUART1('T');
		dist=br/8;
        br=0;
		//WriteUART1((dist%10)+48);
        WriteUART1dec2string(dist);
        WriteUART1(13);

		Delay_us(60000);
        Delay_us(60000);
        Delay_us(60000);

	}
	return 0;
}