#include <string.h>
#include <conio.h>
#include <dos.h>
//definice preruseni casovace
#define INTR_CASOVAC 0X1C


typedef unsigned char byte;

byte slovo=0;
int Cidlo_A=0,Cidlo_B=0,citac=0,limit=20;
signed int akcni_zasah=0;
int rychlost_pozadovana=0;
//int smer_zadany=0;
int smer_mereny=0;
int minCA=0,minCB=0;
int citac_rychlosti=0,rychlost_merena=0,i;
double integral=0;

//oldhandler preruseni casovace
void interrupt ( *OldHandler_Casovac)(__CPPARGS);

void interrupt (*OldHandler_Port)(void);

//handler preruseni casovace
void interrupt Handler_Casovac(__CPPARGS)
{
  signed int odchylka;

  if (citac==limit){citac=0;rychlost_merena=citac_rychlosti;citac_rychlosti=0; }
  else citac++;

//PI regulator
  odchylka=rychlost_pozadovana-rychlost_merena;
  if(integral<-limit){integral=-limit;};
  if(integral>limit){integral=limit;};
  integral=(0.01*odchylka)+integral;
  akcni_zasah=(0.5*odchylka)+integral;
  if (akcni_zasah<-limit){akcni_zasah=-limit;};
  if (akcni_zasah>limit){akcni_zasah=limit;};

//PWM modulace
  if (abs(akcni_zasah)<=citac){outp(0x378,0x00);}
  else

   if (akcni_zasah<0){ outp(0x378,0x01);}
   else outp(0x378,0x02);

/* call the old routine */
  OldHandler_Casovac()
  ;
}




//handler preruseni paralelniho portu
void interrupt Handler_Port(void)
{
  slovo=inp(0x379);    //cteme byte z paralelniho portu
  Cidlo_A=((slovo&0x10)?1:0);  //cteme 4.bit
  Cidlo_B=((slovo&0x20)?1:0);  //cteme 5.bit3

 // Zjisteni smeru
 if((minCA!=Cidlo_A) | (minCB!=Cidlo_B)){

   if ((minCA==0)&(minCB==0)&(Cidlo_A==1)) {smer_mereny=0;citac_rychlosti++;};
   if ((minCA==0)&(minCB==0)&(Cidlo_B==1)) {smer_mereny=1;citac_rychlosti--;};
 minCA=Cidlo_A; minCB=Cidlo_B;
 }


  outp(0x20,0x20);
};

void nastaveni_citace()
{                             //nastaveno 50 Hz
  asm{
      mov al,36h;
      out 43h,al;
      mov al,0x52;
      out 40h,al;
      mov al,0x09;
      out 40h,al;
      }
}

//obnoveni citace po skonceni programu
void obnoveni()
{
 asm{
  mov al,00110110b
  out 43h,al
  mov al,00h
  out 40h,al
  mov al,00h
  out 40h,al
 }
}

unsigned char stisk;
char smer_dol[16]="Doleva";
char smer_dop[16]="Doprava";
char smer_net[16]="Netoci se";
char smer[16];

void main(void) {
  //precteni puvodniho ridiciho registru 37A paralel.portu
  byte old37A=inp(0x37A);
  //precteni puvodniho handleru pro preruseni z paralelniho portu
  OldHandler_Port = getvect(0x0F);
  //nastaveni handleru pro preruseni paralelniho portu
  setvect(0x0F,Handler_Port);
  //povoleni preruseni na radici preruseni
  outp(0x21,inp(0x21) & 0x7F);
  //povoleni preruseni z paralelniho portu
  outp(0x37A,(old37A & ~0x20) | 0x10);
  outp(0x20,0x20);
  //precteni puvodniho handleru pro preruseni casovace
  OldHandler_Casovac = getvect(INTR_CASOVAC);
  //nastaveni handleru pro preruseni casovace
  setvect(INTR_CASOVAC, Handler_Casovac);
  clrscr();
  nastaveni_citace();
   gotoxy(11,6);


  printf("%c",201);
  for(i=0;i<50;i++)
  {
    printf("%c",205);
  }
  printf("%c \n",187);
  gotoxy(62,7);printf("%c \n",186);
  gotoxy(62,8);printf("%c \n",186);
  gotoxy(11,13);printf("%c \n",186);
  gotoxy(62,9);printf("%c \n",186);
  gotoxy(62,10);printf("%c \n",186);
  gotoxy(62,11);printf("%c \n",186);
  gotoxy(11,12);printf("%c \n",186);
  gotoxy(62,12);printf("%c \n",186);
  gotoxy(11,10);printf("%c \n",186);
  gotoxy(62,16);printf("%c \n",186);
  gotoxy(62,13);printf("%c \n",186);
  gotoxy(11,14);printf("%c \n",186);
  gotoxy(62,14);printf("%c \n",186);
  gotoxy(11,15);printf("%c            Klavesy pro ovladani",186);
  gotoxy(62,15);printf("%c \n",186);
  gotoxy(11,16);printf("%c Zvysit rychlost : a 	    Snizit rychlost : z",186);
  gotoxy(11,17);printf("%c Konec : Esc",186);
  gotoxy(62,17);printf("%c \n",186);
  gotoxy(11,18);printf("%c",200);
  for(i=0;i<50;i++)
  {
    printf("%c",205);
  }
  printf("%c",188);




  while (stisk!=27)
   {
     gotoxy(5,5);
    if (kbhit()!=0)
      {
          stisk=getch();
	  if ((stisk=='a')&(rychlost_pozadovana<limit))
	    {
	      rychlost_pozadovana++;
	    }
	  if ((stisk=='z')&(rychlost_pozadovana>-limit))
	    {
	      rychlost_pozadovana--;
            }
          if (stisk=='s')
	    {
	      rychlost_pozadovana=-12;
	    }
      }


   if(rychlost_merena<0)
     {
       strncpy(smer,smer_dol,16);
     }
   else
     if(rychlost_merena!=0)
     {
      strncpy(smer,smer_dop,16);
     }
     else strncpy(smer,smer_net,16);

  gotoxy(11,7);printf("%c Merena rychlost : %02d ",186,rychlost_merena);

  gotoxy(11,8);printf("%c Mereny smer : %s     ",186,smer);

  gotoxy(11,9);printf("%c Akcni zasah : %02d ",186,akcni_zasah);

  gotoxy(11,11);printf("%c        Pozadovana rychlost : %02d ",186,rychlost_pozadovana);

  };


  //obnoveni ridiciho registru paralelniho portu
  outp(0x37A,old37A);
  //zakazani preruseni z portu na radici preruseni
  outp(0x21,inp(0x21) | 0x80);
  //nastaveni puvodniho Handleru pro preruseni z paralelniho portu
  setvect(0x0F,OldHandler_Port);
  //obnoveni nastaveni citace*/
  obnoveni();
  //nastaveni puvodniho Handleru pro preruseni casovace
  setvect(INTR_CASOVAC, OldHandler_Casovac);
  //zastaveni motorku
  outp(0x378,0);
};