;;  trace.lsp -- the Rover traces a line curve on the floor
;;  
;;  Port A: motor for the left wheel of the Rover
;;  Port C: motor for the right wheel of the Rover
;;  Port 1: light sensor for line recognition
;;
;;  Contributor(s): Taiichi Yuasa <yuasa@kuis.kyoto-u.ac.jp>

(define threshold 50)

(define (on-black) (< (light 1) threshold))
(define (on-white) (> (light 1) threshold))

(define (left)
  (motor :c :off)
  (wait-until (on-black))
  (wait-until (on-white))
  (motor :c :back)
  (right))

(define (right)
  (motor :a :off)
  (wait-until (on-black))
  (wait-until (on-white))
  (motor :a :back)
  (left))

(define (start)
  (write-string "Press Prgm button to start")
  (wait-until (pressed?))
  (wait-until (not (pressed?)))
  
  (speed :a (speed :c :max-speed))
  (motor :a (motor :c :back))
  (light-on 1)

  (catch 'end
    (with-watcher (((pressed?) (throw 'end 0)))
      (wait-until (on-black))
      (wait-until (on-white))
      (left)))

  (motor :a (motor :c :off))
  (light-off 1)
  )

(start)