[From Bill Powers (970307.1928 MST)]
The source and executable forms of the new MCT/PCT program, theo6mct.pas and
theo6mct.exe, are on my ftp page at ftp.frontier.net/users/powers_w.
The source code is also appended here.
Best,
Bill P.
···
==================================================================
program theo6mct;
{
This program compares the performance of the MCT and PCT models. Both
models are controlling the position of a theodolite, under the
following conditions:
change of position: 1 radian.
time resolution: 0.01 sec (dt).
Maximum available output torque: 100 newton-meters
Moment of inertia of theodolite: 1 newton-meter^2
Rate of change of reference signal: variable from keyboard
disturbance: 100 n-m, 3 sec duration, random starting time.
The MCT model runs first, in the lower half of the screen. The slope
of the change in reference signal can be adjusted up and down (by
factors of 1.5) by pressing the + and - keys, shifted or unshifted. A
new run is done every time any key is pressed -- except 'p' or 'q'.
Pressing the 'q' key exits the program at any time. Pressing the 'p'
key switches to running the PCT model; after the first time, pressing
'p' or 'm' switches control to the PCT or MCT model, for further
adjustments of the slopes.
The beginning time of the disturbance varies randomly each time a new
run occurs. By pressing the space bar, new runs can be done with the
same value of the slope of the reference signal's rise but a different
start of the disturbance. Thus you can see what happens when there
is or is not a disturbance occurring during the change in reference
signal.
MCT program by Hans Blom; PCT program and presentation by W. T. Powers
4 March 1997
}
uses
dos, crt, graph, grutils;
var
J, K, dt, r, u: real;
a, amod, x, xmod, oldx, oldxmod, v, oldv, vmod, oldvmod: real;
d, dmod, t, pslope,mslope, umax: real;
gv, gx, rv, rx: real;
xplot: integer;
maxx, maxy,ycenter: integer;
ch: char;
numstr: string;
fo: text;
ismct: boolean;
function make_reference (t: real; var slope: real): real;
{this function defines the setpoint at time t}
var ref: real;
begin
if (t >= 3.0) then
begin
ref := slope*(t - 3.0);
if ref > 1.0 then ref := 1.0;
make_reference := ref;
end
else make_reference := 0.0
end;
function true_disturb (t: real): real;
{this function defines the true disturbance}
begin
if (t < 6.0) or (t > 8.0) then
true_disturb := 0.0
else
true_disturb := 50.0;
end;
procedure real_observation;
var savv,savx: real;
{this procedure generates the x that the controller will observe}
begin
savv := v;
savx := x;
d := true_disturb(t);
a := (u + d)/J;
x := x + v*dt + 0.5*a*dt*dt;
v := v + a*dt;
oldv := savv;
oldx := savx
end;
procedure mod_observation;
var savv,savx: real;
{this procedure generates the simulated}
begin
savv := vmod;
savx := xmod;
amod := (u + dmod)/J;
xmod := xmod + vmod*dt + 0.5*amod*dt*dt;
vmod := vmod + amod*dt;
oldvmod := savv;
oldxmod := savx;
end;
procedure plotit(baseline: integer);
begin
xplot := round(50.0*t)+ 180;
putpixel(xplot,baseline,white);
putpixel(xplot,baseline - round(100.0*r),yellow);
putpixel(xplot,baseline - round(100.0*x),white);
putpixel(xplot,baseline - round(d),lightred);
putpixel(xplot,baseline - round(u),lightcyan);
end;
procedure legends;
begin
clearviewport;
setcolor(white);
outtextxy(0,0,'Pointing angle');
setcolor(yellow);
outtextxy(0,15,'Ref level');
setcolor(lightcyan);
outtextxy(0,30,'Output torque');
setcolor(lightred);
outtextxy(0,45,'Disturbance');
end;
Procedure PCTmodel;
begin
ismct := false;
setviewport(0,0,maxx,ycenter,ClipOn);
repeat {REPEAT WHOLE RUN}
t := 0.0; {start at zero time}
d := 0.0; {assume no disturbance initially}
v := 0.0;
x := 0.0;
gv := 100.0;
gx := 100.0;
legends;
setcolor(white);
line(0,ycenter-8,maxx,ycenter-8);
str(pslope:5:3,numstr);
setcolor(white);
outtextxy(0,60,'Slope = '+numstr);
outtextxy(150,100,'PCT MODEL');
umax := 0.0;
repeat {the control loop starts here}
r := make_reference (t,pslope); {define reference}
{CONTROL SYSTEM EQUATIONS}
rv := gx* (r - x); {velocity ref level = output of position control}
u := gv*(rv - v); {output force = output of velocity control}
{ ... which is equivalent to
u:= gv*(gx*(r-x) - v), or
u := gv*gx*(r-x) - gv*v
}
if abs(u) > umax then umax := abs(u);
if u < -100.0 then u := -100.0 else {limit output, if desired}
if u > +100.0 then u := +100.0;
{ENVIRONMENTAL EQUATIONS}
real_observation;
plotit(120);
t := t + dt;
if keypressed then t := 9.1;
until t >= 9.0; {at this point the loop ends}
str(umax:1:0,numstr);
outtextxy(0,75,'Max torque = ' + numstr + ' n-m');
ch := readkey;
if ch in ['=','+'] then pslope := pslope*1.5;
if ch in ['_','-'] then pslope := pslope/1.5;
until ch in ['q','Q','m','M'];
end;
Procedure MCTmodel;
begin
ismct := true;
setviewport(0,ycenter+1,maxx,maxy - 10,ClipOff);
repeat {REPEAT WHOLE RUN}
t := 0.0; {start at zero time}
dmod := 0.0; {assume no disturbance initially}
vmod := 0.0;
xmod := 0.0;
v := 0.0;
x := 0.0;
u := 0.0;
legends;
str(mslope:5:3,numstr);
setcolor(white);
outtextxy(0,60,'Slope = '+numstr);
outtextxy(150,100,'MCT MODEL');
umax := 0.0;
repeat {the control loop starts here}
r := make_reference (t,mslope); {define reference}
{GENERATE REAL X and V}
real_observation;
{GENERATE SIMULATED (predicted) X and V}
mod_observation;
{ESTIMATE DMOD}
dmod := dmod + 2*J/dt/dt*(x - xmod - (oldx - oldxmod)
- (oldv - oldvmod)*dt);
{COMPUTE OUTPUT}
u := J/dt/dt*(r - xmod - vmod*dt) - dmod;
if abs(u) > umax then umax := abs(u);
{LIMIT OUTPUT TO +/- 100 N-M}
if u < -100.0 then u := -100.0 else {limit output, if desired}
if u > +100.0 then u := +100.0;
plotit(125);
t := t + dt;
if keypressed then t := 9.1;
until t >= 9.0; {at this point the loop ends}
str(umax:1:0,numstr);
outtextxy(0,75,'Max torque = ' + numstr + ' n-m');
ch := readkey;
if ch in ['=','+'] then mslope := mslope*1.5;
if ch in ['_','-'] then mslope := mslope/1.5;
until ch in ['q','Q','p','P'];
end;
{initialization}
begin
clrscr; {clear screen}
initgraphics;
maxy := getmaxy;
maxx := getmaxx;
ycenter := (maxy+1) div 2;
J := 1.0; {or whatever value...}
dt := 0.01; {or whatever value...}
K := (J / dt) / dt; {auxiliary constant}
mslope := 0.4;
pslope := 0.4;
setcolor(white);
outtextxy(0,maxy - 10,'q to quit, space to repeat, +/- to change slope');
ch := 'm';
repeat
if ch in ['m','M'] then MCTmodel;
if ch in ['p','P'] then PCTmodel;
until ch in ['q','Q'];
closegraph;
end.