A second implementation of the controller

The splat system provides the constructor hc-blending-navigation-policy which implements a fuzzy combination of the output associated with a set of controllers (control experts). The code below shows how we could use this constructuor to implement the stopping controller.^4.5, ^4.6

(depend 'splat-system)

(define-splat (stopping-controller robot stopping-distance vmax time-stop)
  (local-vars (vset 0) (front-distance #f))
  (initializer (semaphore-wait (base-control-mutex robot)))
  (finalizer (halt robot) (semaphore-signal (base-control-mutex robot)))

  (method 
   #t       ;;;; the method is always applicable

   (define (speed-controller)
     (let* ((v #t) (xt #f) 
            (k (sqrt (/ (* 2 vmax ) time-stop )))
            (xp (sqr (* k (/ time-stop 2.0))))
           )

       (set! xt (max 0 (- front-distance stopping-distance)))
       (set! v (if (> xt xp) vmax  (* k (sqrt xt))))
   ))

   (run
    (hc-blending-navigation-policy 
     (loop-delay 0.1)
     (state-updater 
      (lambda ()
        (bind ((dir dist (min-free-space robot (deg->rad -10) (deg->rad 10))))
              (set! front-distance dist))
        (set! vset (speed-controller))))
     
    (control-expert (lambda () (values 1.0 0 vset )))
    (termination-expert (lambda () (< front-distance  stopping-distance)))
    (control-thunk (lambda (turn drive) (set-drive-and-turn robot drive turn)))

   ))

   (method-finish self 'method-succeeded)
   )
)

The main core of the splat is the hc-blending-navigation-policy which implements a fuzzy combination of the output associated with a set of controllers (control experts). Its general form is

(hc-blending-navigation-policy
 (loop-delay delay-time)
 (state-updater code)
 (control-expert code)... (control-expert code)
 (termination-expert code) ... (termination-expert code)
 (control-thunk code)
)

A hc-blending-navigation-policy returns a splat which is executed until one or more of its termination-experts become true. This type of splat has a built in loop defined as:

 1. Execute the code associated with the state-updater
 2. Execute the code associated with each control-expert
 3. Set output to the fuzzy combination associated of the values returned
    by the different control-experts
 4. Call the code associated with control-thunk with output as a parameter
 5. Sleep delay-time seconds

The state-updater code usually calculates values that are shared by the different experts. Any lambda expression could be the code of a state-updater form.

The code associated with a control-expert should return

 (values applicability rotational-speed forward-speed)

In our example, we only have one control expert and one termination expert. Our control expert always has applicability 1.0, rotational-speed 0 and forward velocity as calculated by the function speed-controller.

Notice that in the body of the splat's method we have the expression (run (hc-blending-navigation-policy ...)) instead of (hc-blending-navigation-policy ...) alone. This latter expression returns (denotes) a splat which is then executed by calling the method run on it.