DECLARE FUNCTION lim! (a!, b!)
SCREEN 9
'VIEW (0, 0)-(639, 349), 15


CONST wbase = 2          ' metres wheelbase
CONST smax = .05         ' maximum radians steering
CONST srate = .016       ' radian per second slew rate
CONST speed = 2.5        ' metres per secomd
CONST focus = 3          ' metres ahead camera viewpoint
CONST fofview = 1        ' field of view, radians
CONST angscale = 640 / fofview  'pixels per radian
CONST xscale = angscale / (wbase + focus)

CONST gain1 = 5          ' angle pixels demand per pixel viewpoint error
CONST gain2 = .00035     ' steering radians per pixel angle error
CONST anglim = 640       ' limit of demanded heading angle, pixels

DIM SHARED x, heading    ' deviation of rear wheels, vehicle angle
DIM SHARED steering      ' in steps
DIM SHARED steertarget   ' target steering position in steps
DIM SHARED angzero, szero

INPUT "Initial error - try 0.1 first, then 0.2 and 0.3"; x
CLS

PRINT "      Initial position:"; x;
COLOR 12: PRINT "  Heading ";
COLOR 14: PRINT "  Steering"
COLOR 15


CONST dmax = 50
WINDOW (-5, -1)-(dmax, 1)
LINE (0, -1)-(0, 1), 9
LINE (0, 0)-(dmax, 0), 9

angzero = 0
szero = 0

dt = .05
angle = 0!
'OPEN "c:\temp\steer2.csv" FOR OUTPUT AS 1
DO
   viewpoint = x + (wbase + focus) * angle
   angseen = (angle + angzero) * angscale

   angdemand = lim(-gain1 * viewpoint * xscale, anglim)
   steertarget = lim(gain2 * (angdemand - angseen), smax)

   dx = angle * speed
   da = (steering + szero) * speed / wbase
   ds = srate * SGN(steertarget - steering)

   dist = dist + speed * dt
   x = x + dx * dt
   angle = angle + da * dt
   steering = steering + ds * dt

   PSET (dist, x), 15
   PSET (dist, (wbase + focus) * angle), 12
'   PSET (dist, viewpoint), 10
   PSET (dist, steering / smax), 14
'   WRITE #1, steering / smax, 5 * angle, x
LOOP UNTIL dist > dmax OR INKEY$ <> ""
CLOSE 1

FUNCTION lim (a!, b!)
IF ABS(a!) < b! THEN lim = a! ELSE lim = b! * SGN(a!)
END FUNCTION