2. Программа расчета параметров настройки пид регулятора

PROGRAM PIDREG;

const M=1.62;

eps=1.0E-06;

type index = array [1..20] of extended;

var X,Ya,Ba,Ca,Da,Yf,Bf,Cf,Df: index;

{ ***************************************************** }

procedure spline(n: integer;x,y: index;var b,c,d: index);

label 1,2,3;

var nm1,ib,i: integer;

t: extended;

begin

nm1:=n-1;

if (n<2) then goto 3;

if (n<3) then goto 2;

d[1]:=x[2]-x[1];

c[2]:=(y[2]-y[1])/d[1];

for i:=2 to nm1 do

begin

d[i]:=x[i+1]-x[i];

b[i]:=2.0*(d[i-1]+d[i]);

c[i+1]:=(y[i+1]-y[i])/d[i];

c[i]:=c[i+1]-c[i]

end;

{ Граничные условия }

b[1]:=-d[1];

b[n]:=-d[n-1];

c[1]:=0.0;

c[n]:=0.0;

if (n=3) then goto 1;

c[1]:=c[3]/(x[4]-x[2])-c[2]/(x[3]-x[1]);

c[n]:=c[n-1]/(x[n]-x[n-2])-c[n-2]/(x[n-1]-x[n-3]);

c[1]:=c[1]*d[1]*d[1]/(x[4]-x[1]);

c[n]:=-c[n]*d[n-1]*d[n-1]/(x[n]-x[n-3]);

{ *************************************** }

1: for i:=2 to n do

begin

t:=d[i-1]/b[i-1];

b[i]:=b[i]-t*d[i-1];

c[i]:=c[i]-t*c[i-1]

end;

c[n]:=c[n]/b[n];

for ib:=1 to nm1 do

begin

i:=n-ib;

c[i]:=(c[i]-d[i]*c[i+1])/b[i];

end;

b[n]:=(y[n]-y[nm1])/d[nm1]+d[nm1]*(c[nm1]+2.0*c[n]);

for i:=1 to nm1 do

begin

b[i]:=(y[i+1]-y[i])/d[i]-d[i]*(c[i+1]+2.0*c[i]);

d[i]:=(c[i+1]-c[i])/d[i];

c[i]:=3.0*c[i]

end;

c[n]:=3.0*c[n];

d[n]:=d[n-1];

goto 3;

2: b[1]:=(y[2]-y[1])/(x[2]-x[1]);

c[1]:=0.0;

d[1]:=0.0;

b[2]:=b[1];

c[2]:=0.0;

d[2]:=0.0;

3: ;

end;

{ ***************************************************** }

function seval(n: integer;u: extended; x,y,b,c,d: index):

extended;

label 1,2,3;

var j,k: integer;

dx: real;

const i: integer=1;

begin

if (i>n) or (i=n) then i:=1;

if (u<x[i]) then goto 1;

if (u<x[i+1]) or (u=x[i+1]) then goto 3;

1: i:=1;

j:=n+1;

2: k:=(i+j) div 2;

if (u<x[k]) then j:=k;

if (u>x[k]) or (u=x[k]) then i:=k;

if (j>i+1) then goto 2;

3: dx:=u-x[i];

seval:=y[i]+dx*(b[i]+dx*(c[i]+dx*d[i]))

end;

{ ***************************************************** }

procedure Intplob(w,A,f: extended; var j: integer);

label 1,2;

var i,k: integer;

const n: integer=1;

begin

if (n=1) then

begin

X[1]:=w;

Ya[1]:=A;

Yf[1]:=f;

end;

if (n>1) then

begin

for i:=1 to n-1 do

begin

if (X[i]<w) then goto 1;

if (X[i]>w) then

begin

for k:=1 to (n-i) do

begin

X[n+1-k]:=X[n-k];

Ya[n+1-k]:=Ya[n-k];

Yf[n+1-k]:=Yf[n-k]

end;

X[i]:=w;

Ya[i]:=A;

Yf[i]:=f;

goto 2

end;

1: ;

end;

if (i=n-1) then

begin

X[n]:=w;

Ya[n]:=A;

Yf[n]:=f;

end

end;

2: j:=n;

spline(j,X,Ya,Ba,Ca,Da);

spline(j,X,Yf,Bf,Cf,Df);

n:=n+1;

end;

{ ***************************************************** }

procedure Virtob(w: real; n: integer; var A,f: real);

begin

A:=seval(n,w,X,Ya,Ba,Ca,Da);

f:=seval(n,w,X,Yf,Bf,Cf,Df)

end;

{ ***************************************************** }

procedure Wsfunpi(n: integer);

label 1,2;

var Kp,Ti,q,g,A,zq,z,w,dw,f,Wp: real;

begin

w:=3.0;

dw:=0.1*w;

Virtob(w,n,A,f);

g:=-f-0.5*Pi;

zq:=M*w*(M*cos(g)-1.0)/A/(sqr(M)-1.0);

1: ;

w:=w+dw;

Virtob(w,n,A,f);

g:=-f-0.5*pi;

q:=M*w*(M*cos(g)-1.0)/A/(sqr(M)-1.0);

Kp:=sqr(M)*sin(g)/(sqr(M)-1.0)/A;

Ti:=Kp/q;

if ( q > zq ) then

begin

zq:=q;

goto 1

end;

if ( q < zq ) then

begin

if ( abs(dw/w) < eps ) then goto 2;

zq:=q;

dw:=-dw/1.62;

goto 1

end;

2: ;

Wp:=w;

writeln(' ');

writeln(' Kp=',Kp:6:4);

writeln(' Tи=',Ti:6:4);

writeln(' Kp/Tи=',Kp/Ti:6:4);

writeln(' Резонансная частота Wp=',Wp:6:4);

end;

{ ***************************************************** }

procedure Nastr;

var w,Am,Gm: extended;

n: integer;

begin

writeln(' ');

writeln(' Введите значение частоты w1');

readln(w);

writeln(' Введите значение амплитуды КЧХ объекта A(w1)');

readln(Am);

writeln(' Введите значение фазы КЧХ объекта F(w1)');

readln(Gm);

Intplob(w,Am,Gm,n);

writeln(' ');

writeln(' Введите значение частоты w2');

readln(w);

writeln(' Введите значение амплитуды КЧХ объекта A(w2)');

readln(Am);

writeln(' Введите значение фазы КЧХ объекта F(w2)');

readln(Gm);

Intplob(w,Am,Gm,n);

writeln(' ');

writeln(' Введите значение частоты w3');

readln(w);

writeln(' Введите значение амплитуды КЧХ объекта A(w3)');

readln(Am);

writeln(' Введите значение фазы КЧХ объекта F(w3)');

readln(Gm);

Intplob(w,Am,Gm,n);

writeln(' ');

writeln(' Введите значение частоты w4');

readln(w);

writeln(' Введите значение амплитуды КЧХ объекта A(w4)');

readln(Am);

writeln(' Введите значение фазы КЧХ объекта F(w4)');

readln(Gm);

Intplob(w,Am,Gm,n);

writeln(' ');

Wsfunpi(n)

end;

{ ***************************************************** }

begin

Nastr

end.