'Press key <F2> to see the text contained in the SUBs'

DECLARE FUNCTION curve! (c!)
DECLARE FUNCTION clip! (c!, d!)
DECLARE SUB axes ()
DECLARE SUB doplot ()
DECLARE SUB showlimits ()
DIM SHARED a$, b$, a, b, umax, x, v, dt, xlim, f
SCREEN 9
VIEW (1, 1)-(638, 348), 8, 14
COLOR , 8
f = 0                        'initial disturbing force, Newtons
cr$ = CHR$(13)
DO
  CLS
  VIEW                         ' Full screen
  WINDOW (-1.2, -6)-(1.2, 6)   ' Position range +/-1.2, velocity +/-6
  axes                         '
  a = 100: b = 100: umax = 10  ' Velocity gain, position gain, drive limit
                               '              (shaped by curve), newtons
                               '
  LOCATE 2, 5
  PRINT "Press number key to change disturbance, return to rerun, space to exit"
  LOCATE 24, 45: PRINT "Disturbing force = "; f; " N";
  showlimits
  dt = .001                    ' Time step of Euler integration
  LOCATE 17, 1                 ' Print settling times from line 17
                               '
                               ' Now plot curves for various steps
  x = -1.5: v = 0
  doplot

  x = -1: v = 0
  doplot

  x = -1: v = 4
  doplot

  x = 1.5: v = 0
  doplot

  x = 1: v = 0
  doplot

  x = 1: v = -4
  doplot

  b$ = ""
  DO
     x = 2 * (RND - .5)
     v = 10 * (RND - .5)
     doplot
  LOOP UNTIL b$ = " " OR b$ = cr$

LOOP UNTIL b$ = " "

SUB axes
LINE (-1, 0)-(1, 0), 9         ' Horizontal
LINE (0, -5)-(0, 5), 9         ' Vertical
LOCATE 3, 38: PRINT "+5"       ' Show limits of axes
LOCATE 14, 6: PRINT "-1"
LOCATE 14, 73: PRINT "+1"
LOCATE 24, 38: PRINT "-5";
END SUB

FUNCTION clip (c, d)
' Apply symmetric limits for drive constraint or piecewise linear control
'
IF ABS(c) > d THEN
   clip = d * SGN(c)
   COLOR 12
ELSE clip = c
   COLOR 15
END IF
END FUNCTION

FUNCTION curve (c)
' Shaping for position error feedback signal
        IF c < -.001 THEN
           curve = -SQR(-10 * c)   ' Parabolic to left
        ELSEIF c < .001 THEN
           curve = 100 * c         ' Linear in narrow central region
        ELSE
           curve = SQR(10 * c)     ' Parabolic to right
        END IF
END FUNCTION

SUB doplot
t = 0
'PRINT "       From x = "; x, "time to 1 mm error = ";
DO
   a$ = INKEY$: IF a$ <> "" THEN b$ = a$     'remember key-press
   IF a$ > " " THEN f = (VAL(a$) - 5)        ' New value of disturbance
   PSET (x, v)                               ' Start of line-segment
   u = clip(-a * v - b * curve(x), umax) + f ' f is the disturbance
   v = v + u * dt                            ' Euler step
   x = x + v * dt                            '
   LINE -(x, v)                              ' Show movement
   t = t + dt                                ' Update time
LOOP UNTIL (ABS(x) < .001 AND ABS(v) < .1)   ' Arrived in limits ?
COLOR 15
'PRINT USING "##.##"; t; : PRINT " sec"       ' Complete the text
END SUB

SUB showlimits
u = umax
FOR i = 1 TO 2
   FOR xx = -1.1 TO 1.1 STEP .01
      PSET (xx, (u - b * curve(xx)) / a)
   NEXT
   u = -umax
NEXT
END SUB