/**************************************************************
Project : PWM
Version : 1.0
Date    : 21.2.2009
Author  : 
Comentar: manualni PWM - melo by se dat ritit pomoci tlacitek
Pouzita serva: HS-422
Chip type           : ATmega128
Program type        : Application
Clock frequency     : 16,000000 MHz
Memory model        : Small
External RAM size   : 0
Data Stack size     : 1024
**************************************************************/
#include <avr/io.h>
#include <avr/iom128.h>
#include <avr/interrupt.h>
//#####################################################################
#define NSTEP 10
#define NDELAY 100 //bylo tu 500
int isdelay = 0;
//######################TLACITKA PRO SERVO 1################################

//###########################INT0 UP###########################
ISR (SIG_INTERRUPT0)
{

if (isdelay)
return;

PORTB=~PORTB; //pro kontrolu funce (ostatni LED na portu B blikaji)

 int tmp=(int)OCR1A;
 if ((tmp+NSTEP)<255)
	OCR1A+=NSTEP;
 else
	OCR1A=255;

	isdelay=1;
}

//###########################INT1 DOWN###########################
ISR (SIG_INTERRUPT1)
{

if (isdelay)
return;

PORTB=~PORTB; //pro kontrolu funce (ostatni LED na portu B blikaji)

 int tmp=(int)OCR1A;
 if ((tmp-NSTEP)<0)
	OCR1A-=NSTEP;
	
 else
	OCR1A=0;

	isdelay=1;
}

//######################TLACITKA PRO SERVO 2################################

//###########################INT2 UP###########################
ISR (SIG_INTERRUPT2)
{

if (isdelay)
return;

PORTB=~PORTB; //pro kontrolu funce (ostatni LED na portu B blikaji)

 int tmp=(int)OCR1B;
 if ((tmp+NSTEP)<255)
	OCR1B+=NSTEP;
	
 else
	OCR1B=255;
    
	isdelay=1;
}


//###########################INT3 DOWN###########################
ISR (SIG_INTERRUPT3)
{

if (isdelay)
return;

PORTB=~PORTB; //pro kontrolu funce (ostatni LED na portu B blikaji)

 int tmp=(int)OCR1B;
 if ((tmp-NSTEP)<0)
	OCR1B-=NSTEP;
 else
	OCR1B=0;

	isdelay=1;
}

//#####################################################################

void preruseni_init(void) // uvodni nastaveni podminek pro povoleni preruseni 
{
  EICRA = 0;
  EIMSK = 0x0f;
  SREG |= (1<<SREG_I);
} 

//#####################################################################

void delay(unsigned int msec);

//#####################################################################

int main(void)
{
  DDRB = 0x61; //povoleni portu PB5 a PB6 jako vystup PWM (OC1A a OC1B)
  DDRD = 0;    //Porty D jako vstupy (interupty nastavene pro tlacitka 1-4)
  PORTD = 0xff;
  preruseni_init();

//######################PWM PRO SERVO 1################################
 
ICR1=20000; //nastavenim ICR1=20000 dosahnu f=50Hz

TCCR1A|=(0<<COM1A0)|(1<<COM1A1)|(0<<COM1B0)
|(0<<COM1B1)|(0<<FOC1A)|(0<<FOC1B)|(1<<WGM11)|(0<<WGM10);
 
TCCR1B|=(0<<ICNC1)|(0<<ICES1)|(1<<WGM13)|(1<<WGM12)
| (0<<CS12)|(1<<CS11)|(0<<CS10);

//######################PWM PRO SERVO 2################################
ICR1=20000; //nastavenim ICR1=20000 dosahnu f=50Hz

TCCR1B|=(0<<COM1B0)|(1<<COM1B1)|(0<<COM1C0)
|(0<<COM1C1)|(0<<FOC1B)|(0<<FOC1A)|(1<<WGM11)|(0<<WGM10);


TCCR1C|=(0<<ICNC1)|(0<<ICES1)|(1<<WGM13)|(1<<WGM12)
| (0<<CS12)|(1<<CS11)|(0<<CS10);


//#####################################################################

  while(1) {
   if (isdelay)
	 delay(NDELAY);
   };
 }

//#####################################################################

//void PWM(unsigned char motor1, unsigned char motor2)
//{
//   OCR1A = motor1; // Vystup servo 1
//   OCR1B = motor2; // Vystup servo 2
//} 

//#####################################################################

void delay(unsigned int msec) {
 int unsigned i,n;
 for (i=msec;i>0;i--) {
  for (n=100;n>0;n--);
 }
 isdelay=0;
}