#include <math.h>
#include <dos.h>
#include <float.h>
#include <stdio.h>
#include <stdlib.h>
#include <graphics.h>
#include <conio.h>
#include <dos.h>
#define Basea 0x280
#define S1 0.0007637
#define S2 0.0002198
double Kx[3]={-85,-80,-100}; /*Kp,*/
double Ka[3]={250,90,500} ;
main()
{
long t1=0;
union REGS inregs,outregs;
int a1,p1;
double aek=0.0,pek=0.0;
double e0[2]={0,0},e1[2]={0,0},esum[2]={0,0},out;
int uk;
inregs.h.ah=2; /*调用DOS中断,inregs是调用前的寄存器值(入口参数),outregs是调用后的寄存器值(返回信息)*/
int86(0x1a,&inregs,&outregs); /*中断*/
biostime(1,0); /*读取设置bios时间*/
outp(0x43,0x36); /*调用0x36参数指定值输出到0x43指定端口并返回该值*/
outp(0x40,0xa9);
outp(0x40,0x04);
for(;;)
{
t1=biostime(0,0);
if(t1>=10)
{
biostime(1,0);
outp( Basea , 20 ); /* 切换 20(摆杆角度)通道并触发 A/D */
do{
;
}
while(! (inp(Basea+5) & 0x80));
a1 = inp(Basea + 1) | (inp(Basea + 2) << 8);
a1 &= 0x0FFF;
outp( Basea , 16 ); /* 切换 16(小车位置)通道并触发 A/D */
do{
;
}
while(! (inp(Basea+5) & 0x80));
p1 = inp(Basea + 1) | (inp(Basea + 2) << 8);
p1 &= 0x0FFF;
aek=(a1-2047)*S1; /*输入*/
pek=(p1-2047)*S2/35*92;
e1[0]=aek; e1[1]=pek;
esum[0]+=e1[0]; esum[1]+=e1[1];
out=Kx[0]*e1[0]+Kx[1]*(e1[0]-e0[0])/0.01+Ka[0]*e1[1]+Ka[1]*(e1[1]-e0[1])/0.01; /*PID控制部分*/
uk=(int)((out+5)/10*4096);
if(uk>4047)
uk=4047;
if(uk<447)
uk=447;
printf("%d\n",uk);
outp(Basea+3,(uk & 0xff));
outp(Basea+4,((uk>>8) & 0xf));
e0[2]=e1[2];
}
}
}
