//______________     vystupy PWM    ______________
void pwmdir(int16_t a){
	if(a>=0)
	{PORTB|=_BV(PIN0);}
	else
	{PORTB&=~_BV(PIN0);}
	OCR1AL=abs(a);
	return;}

void pwm2(int16_t a){
	if(a==0){OCR1AL=0;OCR1BL=0;return;}
	if(a>=0)
	{OCR1BL=0;OCR1AL=abs(a);}
	else
	{OCR1AL=0;OCR1BL=abs(a);}
	return;}

void pwmInAB(int16_t a){
	if(a>=0)
	{PORTB|=_BV(PIN0);PORTB&=~_BV(PIN2);}
	else
	{PORTB|=_BV(PIN2);PORTB&=~_BV(PIN0);}
	OCR1AL=abs(a);
	return;}

void pwmIR(int16_t a){
	OCR1A=abs(a);
	return;}
/*
void pwmNic(int16_t a)
{
return;};
*/
//________________      filtry       ___________
void AD0y(void){
	y=((double)AD[0])/1023;
	return;}

void gpuls(void){
	gtmr++;
	if (gtmr>=gp)
		{gtmr=0;
		 if(gsmer==DOLU)
			{gsmer=NAHORU;gv=gd;}
	  	 else
		 	{gsmer=DOLU;gv=gh;}
		}
	return;}

void fgvw(void){
	w=gv;
	return;}

void fAD1w(void){
	w=((double)AD[1])/1023;
	return;}

void kalman(void){
	//static double y2=0;
	//double r;
	y2=y2+vvv*dt;	//aktualizace stavu
	//v=v;
	
	r=y-y2;		//residuum
	y=y2+K1*r;
	y2=y;if (y2==NAN) y2=0;
	vvv=K2*r+vvv;
	return;}

void zpomal(void){
DDRB|=_BV(PB1);
PORTB|=_BV(PB1);
_delay_ms(13);
return;}
//________________      regulace       ___________

/*
void bezregulace(void)
{return;};
*/
void regulacepokus(void){
	//e1=e;
	e=w-y;
	VI+=e;
			if (VI>maxVI) VI=maxVI;
			if (VI<-maxVI) VI=-maxVI;



akce=Vprp*e+Vitg*VI+Vder*vvv;
//omezeni vystupu:
			if (akce>maxvykon) akce=maxvykon;
			if (akce<-maxvykon) akce=-maxvykon;


Vpwm=akce;
setpwm(Vpwm);
return;}


//________________      mereni       ___________

void freerunAD0(void){
AD[0]=ADC; //mereni
return;}

void freerunAD01(void){
ADMUX|=_BV(MUX0);
AD[0]=ADC; //mereni
ADCSRA|=_BV(ADIF);
while(!(ADCSRA&_BV(ADIF))){};
ADCSRA|=_BV(ADIF);
while(!(ADCSRA&_BV(ADIF))){};
AD[1]=ADC; //mereni
ADMUX&=~_BV(MUX0);
return;}
