Stopping Controller

Next we illustrate a prototypical example of a Splat. We are to implement the stopping controller

 6#6 (4.1)

which stops the robot when x=0. In our case, x represents the distance to a front obstacle minus the distance at which the robot should stop from the obstacle. The value of k can be derived from the robot's maximum forward velocity (vmax) and the period of time during which the robot should disaccelerate before stopping (time-stop). A detailed account of the controller can be found at

$VULCAN/doc/speed-controller.ps.

The Splat associated with (4.1) is as follows:^4.1

(depend 'splat-system)

(define-splat (stopping-controller (robot <mobile-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 
   (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))))
   ))

   (let loop ()
     (bind ((dir dist (min-free-space robot (deg->rad -10) (deg->rad 10))))
           (set! front-distance dist))
     (set! vset (speed-controller))
     (check-and-possibly-quit self)
     (cond
      ((< front-distance  stopping-distance) #t)
      (#t (set-drive-and-turn robot vset 0.0)
          (loop))
   )  
   (method-finish self 'method-succeeded)
   ))
)

The call (depend 'splat-system) makes sure that all the splat relevant code is loaded when the stopping splat is loaded.

The function speed-controller calculates the robot's forward speed according to equation 4.1. In order to calculate the distance to a front obstacle we used the method min-free-space of the <mobile-robot> class .^4.2 This method returns the minimum distance as well as the direction (angle) at which this distance occurs in the 20deg front angular section of the robot.^4.3 In Rscheme, the form bind allows one two refer to the different values returned by a function.

The main body of the splat is a a loop in which we calculate the distance to a front obstacle, the respective speed, and issue the robot's command by calling (set-drive-and-turn robot vset 0.0). The loop finished when (< front-distance stopping-distance) is the case. The important detail in the implementation above is the call (check-and-possibly-quit self)^4.4. This allows the splat system to finish the splat appropriately when required.

Roughly speaking, a Splat is executed by running its initializer and then choosing one applicable method. Once a method succeed (or all of them fail), the splat finalizer is executed, after which the splat is finished. In our example the initializer takes control of the wheelchair base. The finalizer frees the wheelchair base as well as stops the robot. A splat method has the form

(method applicability-predicate code)

where applicability-predicate is a predicate indicating the conditions under which the method is applicable. In our example, the splat has only one method which is always applicable.