#ifndef F_CPU				//jinak v makefile
	#define F_CPU 	11059200
#endif

#include <avr/io.h>
#include <avr/interrupt.h>
#include <avr/wdt.h>
#include <util/delay.h>

/******************definice*****************/

//porty

#define MOD			PINA1
#define LED			PINA2
#define ZVUK		PINA3

#define PWM_R		PINB3
#define UNIV_OUT	PINB5

#define SD			PIND6
#define PWM_L		PIND7



//konstanty

#define PERIODA		0x0FE			//nemuze byt vic jak 0xFF (delka citace)
#define STRED_DOLNI	46				//meze pro paku uprostred
#define STRED_HORNI	54
#define MIN_NAPETI	0x228			//0x228 odpovida 2.7V na vstupu ADC, tzn. 9V pred delicem
#define ZPOMALENI	60				//procenta zpomaleni oproti puvodni max. rychlosti

//makra

#define SETBIT(Adresa, Bit)	(Adresa|=(1<<Bit)) 	//napr. SETBIT(PORTB, PINB2)
#define CLEARBIT(Adresa, Bit)	(Adresa &=~(1<<Bit))
#define CHECKBIT(Adresa, Bit)	(Adresa & (1<<Bit)) //napr. CHECKBIT(PINB, PINB2)

/**********************typy**************/

typedef enum{FALSE, TRUE} boolean_t;
typedef struct {unsigned char h;
				unsigned char l;} motor_t;		//delka high a low PWM signalu, soucet je PERIODA

/************************prototypy funkci************/
void nastav_motory(unsigned char proc_levy, unsigned char proc_pravy); //0 az 100, 50 znamena ze stoji
unsigned char norm(float co);
unsigned char absrozdil(unsigned char a, unsigned char b);
boolean_t stred(float signal);
boolean_t zmer_napeti(void);		//FALSE=malo, TRUE=ok
void preklop_led(void);
void preklop_zvuk(void);

/************************globalni promenne**********/
motor_t mlevy, mpravy;
volatile unsigned long int servo1, servo2, s1, s2;		//11059 pulzu/ms
float fservo1, fservo2;
unsigned char servopulzy1=0, servopulzy2=0;	//cita preteceni citace 1, pri prichodu pulzu nuluju
unsigned char aktualizace=0;
boolean_t nastavit=FALSE;
unsigned char zacatek=0;
boolean_t zablokovano=TRUE;
boolean_t low_bat=FALSE;
unsigned char mereni=0;
unsigned char citacZvuk=0;


/***********************************************/
/***********HLAVNI PROGRAM**********************/
/***********************************************/

int main(void)
{
volatile float smer, plyn;
float mezZvuk=85;

//nastaveni portu
PORTA=0xFF;
DDRA=(1<<LED)|(1<<ZVUK);
PORTB=0x0FF;
DDRB=(1<<PWM_R)|(1<<UNIV_OUT);
PORTC=0x0FF;
PORTD=0X0FF;
DDRD=(1<<SD)|(1<<PWM_L);

CLEARBIT(PORTD, SD);	//zastavim motory

nastav_motory(50, 50);

fservo1=50;	//smer 1.5ms, uprostred
fservo2=50;	//motor 1.5ms, stoji

//spusteni timeru 0 a 2 na PWM
TCCR0=0x03;		//nastaveni casovace0, fosc/64
SETBIT(TIMSK, TOIE0); //a povoleni preruseni od jeho preteceni
TCCR2=0x04;		//nastaveni casovace2, fosc/64
SETBIT(TIMSK, TOIE2); //a povoleni preruseni od jeho preteceni

//spusteni timeru 1, na citani servosignalu
TCCR1B=0x01;	//nastaveni casovace1, hodiny naprimo systemove, mod normal
SETBIT(TIMSK, TOIE1);	//pobezi furt a budu si brat jeho hodnoty a odcitat, pac mam jenom jeden

//nastaveni vnejsich preruseni
GIFR=(1<<INTF0)|(1<<INTF1); //vynulovani priznaku preruseni INT0 a 1
MCUCR=(1<<ISC01)|(1<<ISC11);	//nastaveni spusteni sestupnou hranou
GICR=(1<<INT0)|(1<<INT1);		//a povoleni preruseni

sei();

wdt_enable(WDTO_1S);

while(1)
	{
	if (nastavit)
		{
		wdt_reset();
		fservo1=(float)servo1/110.59-100;			//1ms ..0, 2ms ..100
		fservo2=(float)servo2/110.59-100;
		if ((stred(fservo1) && stred(fservo2)) || zablokovano || low_bat) 
			{
			CLEARBIT(PORTD, SD); 
			CLEARBIT(PORTA, ZVUK);
			}
		else SETBIT(PORTD, SD);	//aby to zbytecne nehucelo			

		plyn=norm(fservo2);
		smer=((-0.01*absrozdil(plyn,50))+1)*(norm(fservo1)-50);
		if (plyn>STRED_DOLNI) nastav_motory(norm(plyn-smer), norm(plyn+smer));	
		else nastav_motory(norm(plyn+smer), norm(plyn-smer));
		
		//nastav_motory(norm(fservo1), norm(fservo2)); //klasicky tankovy rizeni
		
		mezZvuk=(-1.6)*absrozdil(plyn, 50)+85; 

		nastavit=FALSE;
		}

	if (zablokovano) if (zacatek>=100)		//cca 0.5s od zapnuti zapnu motory a rozsvitim LED
		{
		zablokovano=FALSE;
		CLEARBIT(PORTA, LED);
		}

	if (mereni>=200) 
		{
		mereni=0;
		low_bat=!zmer_napeti();
		if (!zablokovano) 
			{
			if (low_bat) preklop_led(); else CLEARBIT(PORTA, LED);
			}
		}
		
	if (citacZvuk>=mezZvuk)
		{
		citacZvuk=0;
		preklop_zvuk();
		}
	}

}


/***********************************************/
/***********KONEC HLAVNIHO PROGRAMU*************/
/***********************************************/

/**************funkce***********/

/************************nastaveni otacek motoru*/
void nastav_motory(unsigned char proc_levy, unsigned char proc_pravy)
{
float proc, zpomal;

if (CHECKBIT(PINA, MOD)) zpomal=ZPOMALENI; else zpomal=100;
proc=50+((zpomal*(proc_levy-50))/100);				
mlevy.l=PERIODA*(1-(proc/100));
mlevy.h=0xFF-(PERIODA-mlevy.l);
mlevy.l=0xFF-mlevy.l;
proc=50+((zpomal*(proc_pravy-50))/100);				
mpravy.l=PERIODA*(1-(proc/100));
mpravy.h=0xFF-(PERIODA-mpravy.l);
mpravy.l=0xFF-mpravy.l;
}

/*************************preruseni od timeru0, levy motor*/
ISR(TIMER0_OVF_vect)
{
if (CHECKBIT(PIND, PWM_L)) 
	{
	CLEARBIT(PORTD, PWM_L);
	TCNT0=mlevy.l;
	}
else
	{
	SETBIT(PORTD, PWM_L);
	TCNT0=mlevy.h;
	}
}

/*************************preruseni od timeru2, pravy motor*/
ISR(TIMER2_OVF_vect)
{
if (CHECKBIT(PINB, PWM_R)) 
	{
	CLEARBIT(PORTB, PWM_R);
	TCNT2=mpravy.l;
	}
else
	{
	SETBIT(PORTB, PWM_R);
	TCNT2=mpravy.h;
	}
}

/*************************preruseni od timeru1, toci furt dokolecka, per. preteceni 0,005925835503472s*/
ISR(TIMER1_OVF_vect)
{
if (servopulzy1<21) servopulzy1++;
if (servopulzy2<21) servopulzy2++;
if ((servopulzy1>20)||(servopulzy2>20)) 
	{
	servo1=16589;	//tzn. nemam signal priblizne 0.12s, 6 period servosignalu
	servo2=16589;
	servopulzy1=0;		//aby sem neskakal furt
	servopulzy2=0;
	}
if (aktualizace>=5) 
	{
	nastavit=TRUE;
	aktualizace=0;
	}
if (zacatek<100) zacatek++;
aktualizace++;
mereni++;
citacZvuk++;
}

/*************************preruseni od servo1 - smer*/
ISR(INT0_vect)
{
unsigned int temp;

GIFR=(1<<INTF0); 	//vymazu priznak
servopulzy1=0;
if (!CHECKBIT(MCUCR, ISC00))
	{
	s1=TCNT1;
	SETBIT(MCUCR, ISC00); //spousteni vzestupnou hranou, tzn. cekam na konec pulzu
	}
else
	{
	temp=TCNT1;
	if (temp>=s1) servo1=temp-s1;
	else servo1=(0xFFFF-s1)+temp;		//tzn. mezitim mi pretek citac
	CLEARBIT(MCUCR, ISC00); //spousteni sestupnou hranou, tzn. cekam na dalsi puls
	}
}

/*************************preruseni od servo2 - rychlost*/
ISR(INT1_vect)
{
unsigned int temp;

GIFR=(1<<INTF1); 	//vymazu priznak
servopulzy2=0;
if (!CHECKBIT(MCUCR, ISC10))
	{
	s2=TCNT1;
	SETBIT(MCUCR, ISC10); //spousteni vzestupnou hranou, tzn. cekam na konec pulzu
	}
else
	{
	temp=TCNT1;
	if (temp>=s2) servo2=temp-s2;
	else servo2=(0xFFFF-s2)+temp;		//tzn. mezitim mi pretek citac
	CLEARBIT(MCUCR, ISC10); //spousteni sestupnou hranou, tzn. cekam na dalsi puls
	}
}

/********************************uprava na rozsah 0 az 100*/
unsigned char norm(float co)
{
if (co<0) return(0);
if (co>100) return(100);
return((unsigned char)co);
}

/*************************************absolutni rozdil*/
unsigned char absrozdil(unsigned char a, unsigned char b)
{
if (a>b) return(a-b); else return(b-a);
}

/*********************************stredova poloha vst. signalu*/
boolean_t stred(float signal)
{
if ((signal>=STRED_DOLNI)&&(signal<=STRED_HORNI)) return(TRUE); else return(FALSE);
}

/*******************************zmeri napeti baterky*/
boolean_t zmer_napeti(void)	
{
ADMUX=0xC0;		//reference AREF, kanal 0
ADCSRA=0x84;
_delay_us(10);
ADCSRA|=0x40;
while ((ADCSRA & 0x10)==0);
ADCSRA|=0x10;
ADCSRA|=0x40;				//pro jistotu dva odmery, prvni nemusi byt ok
while ((ADCSRA & 0x10)==0);
ADCSRA|=0x10;
if (ADCW<=MIN_NAPETI) return(FALSE); else return(TRUE);		
}

/*******************preklopi ledku*/
void preklop_led(void)
{
if (CHECKBIT(PINA, LED)) CLEARBIT(PORTA, LED); else SETBIT(PORTA, LED);
}

/*******************preklopi zvuk. vystup*/
void preklop_zvuk(void)
{
if (CHECKBIT(PINA, ZVUK)) CLEARBIT(PORTA, ZVUK); else SETBIT(PORTA, ZVUK);
}