[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.