#include <string.h>
#include <math.h>
#include <stdlib.h>
#include <stdio.h>
#include <io.h>
#include <dos.h>
#include <conio.h>
#include <bios.h>
#define addr 0x220
typedef struct PID {
float SetPoint;
float Proportion;
float Integral;
float Derivative;
float LastError;
float PrevError;
float SumError;
} PID;
float PIDCalc( PID *pp, float NextPoint )
{
float dError,Error;
Error = pp->SetPoint - NextPoint;
pp->SumError += Error;
dError = pp->LastError - pp->PrevError;
pp->PrevError = pp->LastError;
pp->LastError = Error;
return (pp->Proportion * Error
+ pp->Integral * pp->SumError
+ pp->Derivative * dError
);
}
void PIDInit (PID *pp)
{
memset ( pp,0,sizeof(PID));
}
float EncoderIn (void)
{
unsigned char low,mid,high;
float d_low,d_mid,d_high;
low=inportb(addr+7);
mid=inportb(addr+5);
high=inportb(addr+6);
d_high=high*2.54*32.0;
d_mid=mid*2.54/8.0;
d_low=low*2.54/2048.0;
printf("low=%x,mid=%x,high=%x\n",low,mid,high);
return(4096*2.54-d_high-d_mid-d_mid);
}
void DAOUT(float value)
{
unsigned int digit;
digit=65535*(value+10)/20.0+0x8000;
outportb(addr,digit>>8);
outportb(addr+1,digit);
}
void SetZero(void)
{
outportb(addr+6,0x00);
outportb(addr+4,0x00);
outportb(addr+2,0x80);
}
void main(void)
{
PID sPID;
float rOut;
float rIn;
PIDInit ( &sPID );
sPID.Proportion = 0.5;
sPID.Integral = 0.5;
sPID.Derivative = 0.0;
sPID.SetPoint = 100.0;
SetZero();
getch();
while (!kbhit())
{
rIn = EncoderIn();
rOut = PIDCalc ( &sPID,rIn );
DAOUT( rOut );
printf("L=%f\n",rIn);
}
DAOUT(0);
}