;;  range.lsp -- The Rover moves toward the IR tower
;;  
;;  Port A: motor for the left wheel of the Rover
;;  Port C: motor for the right wheel of the Rover
;;
;;  Contributor(s): Taiichi Yuasa <yuasa@kuis.kyoto-u.ac.jp>

(speed :a (speed :c 100))

(define (start f . args)
  (write-string "Press Prgm button when ready")
  (wait-until (pressed?))
  (wait-until (not (pressed?)))
  (catch 'end
    (with-watcher (((pressed?) (throw 'end 0)))
      (apply f args)))
  (motor :a (motor :c :off))
  )
(define (approach)
  (with-watcher (((touched? 2) (throw 'end 0)))
    (let loop ()
      (look)
      (bugle1) (wait-until (not (playing?)))
      (motor :a (motor :c :forward)) 
      (wait-until (not (linked?)))
      (motor :b (motor :a (motor :c :back)))
      (sleep 10)
      (motor :b :off)
      (loop)))
  )
(define (look)
  (turn-left) (wait-until (not (linked?)))
  (turn-right) (wait-until (linked?))
  (reset-time)
  (wait-until (not (linked?)))
  (turn-left)
  (sleep (+ (/ (time) 2) 4))
  (motor :a (motor :c :off)) 
  )
(define (turn-right)
  (motor :a :forward) (motor :c :back)
  )
(define (turn-left)
  (motor :a :back) (motor :c :forward)
  )

;; this code does not work any more (03/12/14)

(define (bugle)
  (play '((:Do4 . 2) (:Do4 . 1) (:Do4 . 2) (:Do4 . 1) (:Do4 . 2) (:Do4 . 1)
          (:So4 . 2) (:Mi4 . 1) (:So4 . 2) (:Mi4 . 1) (:So4 . 2) (:Mi4 . 1)
          (:Do4 . 2) (:Do4 . 1) (:Do4 . 2) (:Do4 . 1) (:Do4 . 2) (:Do4 . 1)
          (:So4 . 2) (:Mi4 . 1) (:So4 . 2) (:Mi4 . 1) (:So4 . 2) (:Mi4 . 1)
          (:Do4 . 6)))
  )
(define (bugle1)
  (play '((:So3 . 1) (:So3 . 1) (:Do4 . 2) (:Do4 . 2) (:Do4 . 2)
          (:So3 . 1) (:Do4 . 1) (:Mi4 . 2) (:Mi4 . 2) (:Mi4 . 2)
          (:Do4 . 1) (:Mi4 . 1) (:So4 . 2) (:Mi4 . 1) (:Do4 . 1)
          (:So4 . 2) (:Mi4 . 1) (:Do4 . 1)
          (:So3 . 2) (:So3 . 1) (:So3 . 1) (:So3 . 2)))
  )