Display servers

Next we illustrate how the right wall following controller defined in Section A.1.2 (page A.1.2) can be enhanced to display the value of the error, orientation, and turning command, while running. In order to do this we use the labview vi depicted in Figure A.1.^5.3

  

Figure: Lavbiew display of error and orientation.

19#19

In order to update the display, three lines are added to the original controller as shown below:

(define-splat (bf-controller robot setpoint ke kth v)
 (local-vars (e 0) (th 0) (init-state-covariance #f) (omega 0)
             (kf (boundary-following::set-kalman-filter ke kth v))
 )
 (finalizer (halt robot))

 (method 
    #t

    (define (current-e-theta)
       (bind ((ang r (min-free-space robot (deg->rad -120) (deg->rad -60))))
          (set! e (- r setpoint))
          (set! th (- (deg->rad -90) ang ))))

    ;; set initial estimates of error and thetha and initialize the 
    ;; kalman filter

    (set! init-state-covariance  (matrix:create 2 2 0))
    (matrix:set! init-state-covariance 1 1 (sqr (deg->rad 2)))
    (matrix:set! init-state-covariance 2 2 (sqr 0.2))
    (current-e-theta)
    (start kf (boundary-following::make-reading e th) init-state-covariance)
    
    ;; follow the wall

    (let loop ()
      (current-e-theta)
      (project kf (boundary-following::make-reading e th))
      (set! omega 
            (* (/ -1.0 (max 0.01 v))
               (+ (* ke (matrix:ref (state kf) 2 1)) 
                  (* kth v (matrix:ref (state kf) 1 1)))))

      ;;; new lines: update the display

      (update-var *mc-handle* 'error e)
      (update-var *mc-handle* 'theta (rad->deg th))
      (update-var *mc-handle* 'omega (rad->deg omega)) 

      (check-and-possibly-quit self)
      (set-drive-and-turn robot v omega)
      (loop))
  )
)

As explained in Section A.1.2 (page A.1.2), one has to define the variables and the rules associated with the display. Here is the code:^5.4

(define (controller-display host port)
 (let ((dis (make <display-server> name: ``display'' port: port host: host)))
   (start dis)
   (add-var dis (make <display-var> name: 'error))
   (add-var dis (make <display-var> name: 'theta))
   (add-var dis (make <display-var> name: 'omega))

   (add-rule dis 'error
     (lambda (x s) (write-socket-lf (sock s) (format #f "error ~a" x))))
   (add-rule dis 'theta
     (lambda (x s) (write-socket-lf (sock s) (format #f "theta ~a" x))))
   (add-rule dis 'omega
     (lambda (x s) (write-socket-lf (sock s) (format #f "omega ~a" x))))
   dis
 )
)

In order to run this example, run labview, and then run the VI $VULCAN/examples/error-theta.vi. Load the sources: $VULCAN/examples/kalman-filter, $VULCAN/examples/righ-wall-bf-display-server. Star the display, and run the controller as shown below:

top[2]=>(set-display-server! *mc-handle* (controller-display ``glare'' 6900))
top[3]=> (set! mc (bf-controller my-robot  2 0.15 (sqrt (* 0.15 4)) 0.5))
top[4]=> (start-task mc #f)
....
top[5] => (finish mc 'succeed)