[From Bill Powers (970306.1034 MST)]
Hans Blom (970306) --
Since you're busy, I thought I'd try my hand at fixing the MCT model to use
the correct equation for the theodolite response. This works, but there's
some transient oscillation, so I may have the indices off by one step either
way. Perhaps you can find time to check them over, now that the main part of
the program is fixed. The program, theo6MCT, is appended at the end.
The derivation is as follows.
MODEL OF THEODOLITE:
x := x + v*dt + 0.5*a*dt*dt;
v := v + a*dt;
The values of xold and vold are saved using your method. v is automatically
the "new old" value of v when the next iteration comes around, since it's
computed _after_ the equation that uses it.
CALCULATION OF U:
x(t+dt) = x(t) + v(t)*dt + 0.5*(u+d)/J)*dt*dt;
assert x(t+dt) = r(t+dt), using estimated disturbance and
true velocity, and dropping index. It is assumed that velocity, like
position, is sensed. We then derive
r = x + v*dt + 0.5*(u+d)*dt*dt/J;
(u+d) = 2*J/dt/dt*([r- x - v*dt];
Program step (d is estimated disturbance):
u := 2*J/dt/dt*(r - x - v*dt);
PREDICTION OF X:
Uses same equation above:
x(t+dt) = x(t) + v(t)*dt + 0.5*(u+d)/J)*dt*dt;
CORRECTION OF D:
Subtract true x (tx) from predicted x (px), to find difference between
predicted d (pd) and true d (td)k, using true velocity tv and predicted
velocity pv:
px - tx = (pv - tv)*dt + (u+pd - u-td)*dt*dt/J
which yields, I think,
pd - td = 2*J/dt/dt*[(px - dx) - (pv-tv)*dt], or as a program step,
d := d - 2*J/dt/dt*(px-tx - (pv-tv)*dt);
In the program I use slightly different symbols (trued for td, xpre for px,
etc.). I'm not sure I've used the "old" and "current" values of variables
correctly in the right places. Can you check this out?
Program follows.
Best,
Bill P.
···
a := (u + trued)/J;
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, x, u, a, v: real;
xold, vold, xpre, vpre, xsav, vsav: real;
d, d1, d2, trued, 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 do_observation;
var a: real;
{this function generates the x that the controller will observe}
begin
trued := true_disturb(t);
a := (u + trued)/J;
xsav := x; {save present x}
vsav := v;
x := x + v*dt + 0.5*a*dt*dt;
v := v + a*dt;
xold := xsav; {xold := previous x}
vold := vsav;
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(trued),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 := 50.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}
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}
do_observation;
plotit(120);
t := t + dt;
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}
xold := 0.0; {start at zero position}
vold := 0.0;
x := xold; {and at zero velocity}
v := vold;
d := 0.0; {assume no disturbance initially}
v := 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}
{COMPUTE OUTPUT}
u := J/dt/dt*(r - x - v*dt);
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;
{GENERATE PREDICTED X}
a := (u+d)/J;
xpre := x + v*dt + 0.5*a*dt*dt;
vpre := v + a*dt;
{ENVIRONMENTAL EQUATIONS}
do_observation; {calculate next x and next v}
plotit(125);
t := t + dt;
{ESTIMATE DISTURBANCE}
d := d - 2*J/dt/dt * ((xold - xpre) - (vold - vpre)*dt);
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.