[Date Prev][Date Next][Thread Prev][Thread Next][Date Index][Thread Index]

MCL floating point bug? code



Sorry, the code in my last posting
should be replaced with the following code.


(let
  ((left-speed -10)
   (right-speed 10)
   (width 12)
   (theta 0.0))
  (multiple-value-bind (delta-x delta-y delta-theta)
                       (cond 
                        ((and (zerop left-speed) (zerop right-speed))
                         (values 0 0 0))
                        ((= left-speed right-speed)
                         (let ((amount (* 0.05 left-speed)))
                           (values (* amount (cos theta))
                                   (* amount (sin theta))
                                   0)))
                        (t (let* ((c (cond ((zerop left-speed) (- width))
                                           ((zerop right-speed) width)
                                           (t (let ((rho (/ right-speed left-speed)))
                                                (* width (/ (+ rho 1.0) (- 1.0 rho)))))))
                                  (sgn (if (<= c 0)
                                         (if (> right-speed 0) 1 -1)
                                         (if (> left-speed 0) -1 1)))
                                  (delta-theta (* sgn 
                                                  #.(/ (* 0.6 pi) 180.0)
                                                  (abs (/ (if (zerop right-speed) left-speed right-speed)
                                                          (- width c)))))
                                  (new-theta (+ delta-theta theta)))
(format t "~%delta-theta = ~A" delta-theta)
                             (values (* c (- (cos theta) (cos new-theta)))
                                     (* c (- (sin theta) (sin new-theta)))
                                     delta-theta)