#include #include #include //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=='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); };