;;  remote.lsp -- controls the Rover with wired remote controller
;;  
;;  Port A: motor for the left wheel of the Rover
;;  Port C: motor for the right wheel of the Rover
;;  Port 1: rotation sensor for the driving wheel of the controller
;;  Port 3: touch sensor for the shift changer of the controller
;;
;;  Contributor(s): Taiichi Yuasa <yuasa@kuis.kyoto-u.ac.jp>

(let ((dir :off))
  ;; attach rotation sensor to port 1.
  ;; attach touch sensor to port 3.
  (speed :a (speed :c (speed :b :max-speed)))
  (rotation-on 1)
  (let loop ()
    (if (touched? 2)
      	(begin
      	  (motor :a (motor :c (motor :b :off)))
      	  (rotation-off 2))
      	(begin
      	  (if (touched? 3)
      	      (begin
      	        (set! dir
      	          (if (= dir :off) :forward
      	            (if (= dir :forward) :brake
      	              (if (= dir :brake) :back :off))))
      	        (motor :b (if (= dir :back) :back :off))
      	        (motor :a (motor :c dir))
      	        (rotation-on 1)
      	        (sleep 3)))
      	  (let ((rot (rotation 1)))
      	    (if (>= rot 0)
      	      (begin
      	        (speed :a
      	          (- :max-speed
      	            (/ (* :max-speed (if (> rot 4) 4 rot)) 4)))
      	        (speed :c :max-speed))
      	      (begin
      	        (speed :c
      	          (- :max-speed
      	            (/ (* :max-speed (if (< rot -4) 4 (- rot))) 4)))
      	        (speed :a :max-speed))))
      	  (loop)))))