[From Bill Powers (990829.1334 MDT)] {My 73rd birthday}
In this post are included two routines: mouse.pas, and motion.pas.
Mouse.pas is a Unit and should be compiled so Mouse.tpu appears in the
directory where Units are kept. Motion.pas is a program that tests the
control of the speed and direction of movement of an individual; the source
code is well-annotated.
I realize that speed and direction control could be much simplified; once
we see how two control systems can be used, we could simply say that the
speed equals the reference speed and the direction equals the reference
direction, without actually simulating the control systems that accomplish
this result. If we have a problem with the execution speed, later on, we
can make this simplification. However, let's keep the explicit control
systems for now, for their teaching value.
Best,
Bill P.
···
===========================================================================
unit mouse;
interface
uses dos,crt;
var mousex,mousey: integer;
function initmouse: boolean;
procedure readmouse;
function readbutton: integer;
implementation
var MouseR : registers;
dx,dy: real;
{ ---------------------- Mouse Functions -----------------------------------}
function initmouse: boolean;
begin { false if mouse not found }
mousex := 0; mousey := 0;
dx := 0; dy := 0;
MouseR.ax := 0;
intr ($33, mouser);
if Mouser.ax <> $ffff then
begin
writeln('MOUSE NOT INSTALLED');
delay(1000);
end;
Initmouse := (MouseR.ax = $ffff);
end;
procedure readmouse;
begin
mouser.ax := 11;
intr ($33, mouser);
dx := dx + 0.7*(integer(MouseR.cx) - dx);
dy := dy + 0.7*(integer(MouseR.dx) - dy);
mousex := mousex + round(dx);
mousey := mousey - round(dy);
end;
function readbutton: integer; { returns 1,2,or 4}
begin
MouseR.ax := 3;
intr ($33, MouseR);
readbutton := MouseR.bx and 3;
end;
end. { of unit }
program motion;
{This program is a test of two control systems for the Crowdv3 development,
both contained in the procedure MovePerson.
There are two reference signal inputs, one for speed and the other for
direction. The mouse is used to adjust the reference signals independently:
y mouse position determines speed, x mouse position determines direction.
Later one or more higher-level control systems will adjust these
reference signals. This code segment will be used for all active persons.
For now there is just one active person.}
uses dos,crt,graph,setSVGA,mouse;
const dt = 0.001;
type activetype = record
FwdForce, RefFwdVel, FwdVel, FwdGain,
SideForce, RefDir, Dir, SideGain,
X, Y, Xaccel, Yaccel, Xvel, Yvel,
dx, dy: double;
mass: double;
end;
var ch : char;
i: integer;
numstr: string;
iter: longint;
{NOTE: the following record will be expanded into a list
of all active persons when there is more than one}
activeperson: activetype;
procedure initprogram;
begin
initmouse; {initialize mouse}
initSVGA(4); {Initialize screen to 800 x 600}
with activeperson do
begin
x := 0; {meters}
y := 0; {meters}
dir := 0; {radians}
FwdVel := 0; {m/sec}
mass := 75; {kg}
FwdGain := 50.0; {speed control gain}
SideGain := 50.0;{direction control gain}
end;
mousex := 0;
mousey := 0;
setfillstyle(0,0);
iter := 0;
end;
procedure shownum(x:double);
begin
str(x:8:4,numstr);
bar(0,10,100,20);
outtextxy(0,10,numstr);
end;
{NOTE: this is not used yet}
function invtan(x1,y1,x2,y2: double): double;
var dx,dy: double;
begin
dx := x2 - x1; dy := y2 - y1;
if dy = 0.0 then invtan := 0.0
else if dx = 0.0 then invtan := pi/2.0
else
invtan := arctan(dy/dx);
end;
{Notes on MovePerson:
Forward velocity (vel) is integral of forward acceleration, or
vel = vel + (force/mass)*dt.
Sideward force is mass*vel^2/radius (equation for centrifugal force),
so radius = mass*vel^2/force.
Rate of change of direction is
d(dir)/dt = vel/radius
Thus d(dir) = vel/(mass*vel^2/force)*dt
= force/(mass*vel)*dt
}
procedure MovePerson;
var dS,Accel: double;
begin
with activeperson do
begin
{Mouse sets reference levels for Forward Velocity and Direction}
RefFwdVel := 0.0005 * mousey;
RefDir := -0.01 * mousex;
{SPEED CONTROL:
Sensed forward velocity is compared to reference forward velocity (user-
controlled). Forward force is the output, proportional by FwdGain to
the speed error.}
FwdForce := FwdGain * (RefFwdVel - FwdVel);
{Forward acceleration is proportional to force divided by mass}
Accel := FwdForce/Mass;
{Distance traveled during time dt is the velocity at the start of the
time interval times dt plus 1/2 the acceleration times the square of
the time interval. Note use of parenthesis to save one multiplication}
dS := (FwdVel+ 0.5 * accel * dt) * dt;
{Project change in velocity onto x and y axes, using angle of travel.
Add distance traveled to update X and Y position.}
X := X + dS * cos(dir);
Y := Y + dS * sin(dir);
{Update Forward Velocity}
FwdVel := FwdVel + accel * dt;
{DIRECTION CONTROL:
Sideward force is proportional by SideGain to the direction error}
SideForce := SideGain * (RefDir - Dir);
{Prevent division by zero error}
if FwdVel > 0.01 then
{Rate of change of direction is the sideward acceleration divided by
the forward velocity -- see "Notes on MovePerson" above. New direction
is old direction plus rate of change of direction times dt }
dir := dir + SideForce/(mass * FwdVel) * dt;
inc (iter);
if (iter mod 100) = 0 then shownum(FwdVel);
end;
end;