1 ; File: "robot.scm", Time-stamp: <2006-03-01 15:57:44 feeley>
3 ; Copyright (C) 2006 by Marc Feeley, All Rights Reserved.
5 ; usage: usage: robot [[BASE_HOSTNAME|.] ID [HEX_FILE]]
9 ;------------------------------------------------------------------------------
11 (define default-base "localhost") ; internet address of base-station server
12 (define port-number 12345)
14 ;------------------------------------------------------------------------------
16 (define version-addr 6)
17 (define program-filename "robot.hex")
18 (define program-start-addr #x2000)
20 (define serial-port-name "com1") ; default, works for Windows
22 (let loop ((lst '("/dev/cu.USA28X181P1.1"
23 "/dev/cu.USA28X181P2.2"
24 "/dev/cu.USA28X191P1.1"
25 "/dev/cu.USA28X191P2.2"
29 (let ((name (car lst)))
30 (if (file-exists? name)
31 (set! serial-port-name name)
34 ;------------------------------------------------------------------------------
38 (with-exception-catcher
42 (open-output-file (list path: "robot.log" buffering: 'line))))))
44 ;------------------------------------------------------------------------------
46 (current-user-interrupt-handler exit)
51 (display "usage: robot [[BASE_HOSTNAME|.] ID [HEX_FILE]]\n"))
53 (define (parse-arg1 args)
56 (let ((arg (car args)))
57 (if (exact-int? (string->number arg))
58 (parse-arg2 default-base args)
59 (parse-arg2 arg (cdr args))))))
61 (define (parse-arg2 base args)
64 (let ((arg (string->number (car args))))
65 (if (and (exact-int? arg)
68 (parse-arg3 base arg (cdr args))
71 (define (parse-arg3 base id args)
73 (parse-arg4 base id #f)
74 (let ((arg (car args)))
75 (if (null? (cdr args))
76 (parse-arg4 base id arg)
79 (define (parse-arg4 base id filename)
81 (start-client base id filename program-start-addr)
86 (define (exact-int? x)
87 (and (integer? x) (exact? x)))
91 (let ((connection-queue
93 (list port-number: port-number
95 eol-encoding: 'cr-lf))))
97 (serve (read connection-queue))
100 (define (start-client base id filename start-addr)
101 (set! program-start-addr start-addr)
103 (if (string=? base ".")
104 (receive (client server) (open-string-pipe)
109 (list server-address: base
110 port-number: port-number
111 eol-encoding: 'cr-lf)))))
113 (with-exception-catcher
117 (host-info-name (host-info "")))))
119 (let ((ack (read connection)))
120 (if (equal? ack '(ok))
124 (set! program-filename filename)
125 (start-client-upload connection id)))
126 (start-client-console connection id))
128 (list "Another client is already connected to robot " id "\n"))))
129 (close-port connection)))
131 (define (start-client-upload connection id)
132 (let ((mem (read-hex-file program-filename)))
134 (upload connection id mem program-start-addr))))
136 (define (start-client-console connection id)
138 (define (restart-robot)
139 (restart connection id)
140 (start-client-console connection id))
142 (define (upload-again)
143 (start-client-upload connection id)
144 (start-client-console connection id))
148 (start-client-console connection id))
151 (list "###\n### Console:\n"))
152 (let ((input (repl-input-port)))
154 (tty-mode-set! input #t #t #t #f 0)))
155 (let ((input (repl-input-port))
159 (let loop1 ((state 0))
160 (input-port-timeout-set! connection 0.01)
161 (let ((x (read connection)))
162 (if (not (eof-object? x))
163 (cond ((or (eq? x 'err)
165 (set! can-send-key #t))
169 (if (and (>= (u8vector-length x) 3)
170 (= (quotient (u8vector-ref x 0) nb-ids)
172 (let ((seq-num (u8vector-ref x 1)))
173 (if (not (= seq-num rx-seq-num))
175 (set! rx-seq-num seq-num)
177 (if (< i (u8vector-length x))
178 (let ((n (u8vector-ref x i)))
183 ((or (< n 32) (> n 126))
187 (write-char (integer->char n))))
188 (loop2 (+ i 1))))))))
189 (write (u8vector->list x))))))))
192 (input-port-timeout-set! input 0.01)
193 (let ((x (read-char input)))
197 (vector 'send-message
198 (+ id (* nb-ids MSG_TYPE_STDIO))
202 (set! can-send-key #f)
203 (set! tx-seq-num (modulo (+ tx-seq-num 1) 256))
207 (cond ((char=? x #\tab)
211 (cond ((char=? x #\u001b)
216 (cond ;((char=? x #\u001b))
224 (cond ((char=? x #\P) ; F1
235 (cond ((char=? x #\A)
248 ;------------------------------------------------------------------------------
250 (define (read-hex-file filename)
252 (define addr-width 32)
254 (define (syntax-error)
255 (error "Improper HEX file"))
258 (with-exception-catcher
262 (open-input-file filename)))))
264 (define mem (make-vector 16 #f))
266 (define (mem-store! a b)
269 (x (- addr-width 4)))
272 (let ((i (arithmetic-shift a (- x))))
273 (let ((v (vector-ref m i)))
275 (let ((v (make-vector 16 #f)))
278 (- a (arithmetic-shift i x))
283 (define (f m a n tail)
285 (define (g i a n tail)
287 (g (- i 1) (- a n) n (f (vector-ref m i) a n tail))
292 (cons (cons (- a 1) m) tail)
293 (g 15 a (quotient n 16) tail))
296 (f mem (expt 2 addr-width) (expt 2 addr-width) '()))
301 (define (read-hex-nibble)
302 (let ((c (read-char f)))
303 (cond ((and (char>=? c #\0) (char<=? c #\9))
304 (- (char->integer c) (char->integer #\0)))
305 ((and (char>=? c #\A) (char<=? c #\F))
306 (+ 10 (- (char->integer c) (char->integer #\A))))
307 ((and (char>=? c #\a) (char<=? c #\f))
308 (+ 10 (- (char->integer c) (char->integer #\a))))
312 (define (read-hex-byte)
313 (let* ((a (read-hex-nibble))
314 (b (read-hex-nibble)))
320 (let ((c (read-char f)))
321 (cond ((not (char? c)))
322 ((or (char=? c #\linefeed)
325 ((not (char=? c #\:))
328 (let* ((len (read-hex-byte))
331 (type (read-hex-byte)))
332 (let* ((adr (+ a2 (* 256 a1)))
333 (sum (+ len a1 a2 type)))
337 (let ((a (+ adr (* hi16 65536)))
340 (set! adr (modulo (+ adr 1) 65536))
349 (let* ((a1 (read-hex-byte))
350 (a2 (read-hex-byte)))
351 (set! sum (+ sum a1 a2))
352 (set! hi16 (+ a2 (* 256 a1)))))
355 (let ((check (read-hex-byte)))
356 (if (not (= (modulo (- sum) 256) check))
358 (let ((c (read-char f)))
359 (if (or (not (or (char=? c #\linefeed)
360 (char=? c #\return)))
369 (list "\n### The file " filename " does not exist\n"))
372 (define (upload connection id mem start-addr)
374 (define max-programmable-address 65535)
376 (define bp 8) ; program block size
377 (define be 64) ; erase block size
379 (if (start-programming connection id)
381 (let loop1 ((last-erased-be -1)
386 (a-bp (quotient a bp))
387 (a-be (quotient a be))
388 (bp-bytes (make-u8vector bp 255)))
389 (if (<= a max-programmable-address)
390 (if (or (= a-be last-erased-be)
391 (let ((a (* a-be be)))
393 (erase-block connection id a))))
395 (u8vector-set! bp-bytes (modulo a bp) (cdr x))
396 (let loop2 ((lst2 (cdr lst)))
397 (if (and (pair? lst2)
398 (let ((a (car (car lst2))))
399 (and (<= a max-programmable-address)
400 (= (quotient a bp) a-bp))))
402 (u8vector-set! bp-bytes
403 (modulo (car (car lst2)) bp)
406 (if (let ((a (* a-bp bp)))
408 (program-block connection id a bp-bytes)))
411 (reboot connection id)))
412 (reboot connection id))))))
414 (define (request cmd connection)
419 (let ((x (request-once cmd connection)))
428 (display " ERROR!\n")
431 (define (request-once cmd connection)
432 (send cmd connection)
434 (let ((x (read connection)))
435 (cond ((or (eq? x 'err)
441 (define (request-version connection id)
443 (define (version-msg? version)
444 (and (u8vector? version)
445 (= (u8vector-length version) 5)
446 (= (u8vector-ref version 1)
447 (quotient version-addr 256))
448 (= (u8vector-ref version 2)
449 (modulo version-addr 256))))
452 (input-port-timeout-set! connection #f)
455 (request-once (vector 'set-program-mode id) connection)
457 (vector 'send-message
458 (+ id (* nb-ids MSG_TYPE_PROGRAM))
459 (u8vector (quotient version-addr 256)
460 (modulo version-addr 256)
463 (input-port-timeout-set! connection 1)
466 (let ((x (read connection)))
467 (cond ((eof-object? x)
481 (loop ack version)))))))
483 (define (send obj port)
488 (define (start-programming connection id)
490 (list "\n### Programming robot " id " with " program-filename))
491 (enter-program-mode connection id))
493 (define (stop connection id)
495 (list "\n### Stopping robot " id))
496 (enter-program-mode connection id))
498 (define (restart connection id)
500 (list "###\n### Connecting to robot " id))
501 (enter-program-mode connection id)
502 (reboot connection id))
504 (define (enter-program-mode connection id)
509 (let ((version (request-version connection id)))
511 (let ((version-major (u8vector-ref version 3))
512 (version-minor (u8vector-ref version 4)))
513 (if (and (= version-major 1)
519 (display " INCOMPATIBLE FIRMWARE!\n")
523 (display " THE ROBOT IS NOT RESPONDING!\n")
526 (define (erase-block connection id addr)
527 ; (set! addr (+ addr #x2000))
529 (list "###\n### Erasing block 0x"
530 (number->string addr 16)))
532 (vector 'send-message
533 (+ id (* nb-ids MSG_TYPE_PROGRAM))
534 (u8vector (quotient addr 256)
538 (define (program-block connection id addr bytes)
539 ; (set! addr (+ addr #x2000))
541 (list "### Programming block 0x"
542 (number->string addr 16)))
544 (vector 'send-message
545 (+ id (* nb-ids MSG_TYPE_PROGRAM))
547 (u8vector (quotient addr 256)
552 (define (reboot connection id)
554 (list "###\n### Restarting robot"))
556 (vector 'send-message
557 (+ id (* nb-ids MSG_TYPE_PROGRAM))
561 ;------------------------------------------------------------------------------
568 (define multiplexer #f)
573 (set! mutex (make-mutex))
574 (set! clients (make-vector nb-ids #f))
575 (set! multiplexer (open-vector))
579 (list path: serial-port-name
580 eol-encoding: 'cr-lf)))
582 (tty-mode-set! rs232 #f #f #t #t 38400))
591 (input-port-timeout-set! multiplexer 0.01)
593 (let ((x (read multiplexer)))
595 (let* ((id (vector-ref x 0))
596 (cmd (vector-ref x 1))
597 (cmd-type (vector-ref cmd 0)))
598 (cond ((eq? cmd-type 'send-message)
599 (let ((dest (vector-ref cmd 1))
600 (bytes (vector-ref cmd 2)))
601 (if (send-message dest bytes)
602 (let ((s (wait-until-end-of-tx)))
606 ((not (= (bitwise-and s NOERR_MASK) 0))
607 (ir-tx-event-noerr-ack)
608 (send-to-id 'noerr id)
610 ((not (= (bitwise-and s ERR_MASK) 0))
611 (ir-tx-event-err-ack)
620 ((eq? cmd-type 'set-program-mode)
621 (let ((dest (vector-ref cmd 1)))
622 (if (set-program-mode dest)
623 (let ((s (wait-until-end-of-tx)))
627 ((not (= (bitwise-and s NOERR_MASK) 0))
628 (ir-tx-event-noerr-ack)
629 (send-to-id 'noerr id)
631 ((not (= (bitwise-and s ERR_MASK) 0))
632 (ir-tx-event-err-ack)
633 (send-to-id 'noerr id)
644 (poll-status-handling-rx)
647 (define (set-program-mode dest)
648 (let ((s (prepare-to-tx)))
650 (let ((b (+ dest (* nb-ids MSG_TYPE_SET_PROG_MODE))))
651 (ir-tx-special (- #xff b) b)))))
653 (define (send-message dest bytes)
654 (let ((s (prepare-to-tx)))
658 (list "sending to " (modulo dest nb-ids) ": "))
659 (write (u8vector->list bytes))
661 (ir-tx dest bytes)))))
663 (define (prepare-to-tx)
665 (let ((s (wait-until-end-of-tx)))
668 ((not (= (bitwise-and s NOERR_MASK) 0))
669 (ir-tx-event-noerr-ack)
671 ((not (= (bitwise-and s ERR_MASK) 0))
672 (ir-tx-event-err-ack)
677 (define (wait-until-end-of-tx)
679 (let ((s (poll-status-handling-rx)))
682 ((not (= (bitwise-and s TX_MASK) 0))
687 (define (poll-status-handling-rx)
689 (let ((s (poll-status)))
692 ((not (= (bitwise-and s RX_MASK) 0))
698 (define (handle-rx-message)
701 (let ((id (modulo (u8vector-ref msg 0) nb-ids)))
703 (list " received from " id ": "))
704 (write (u8vector->list msg))
706 (send-to-id msg id)))))
708 (define (send-to-id msg id)
710 (let ((client (vector-ref clients id)))
712 (with-exception-catcher
714 (vector-set! clients id #f))
716 (send msg client)))))
717 (mutex-unlock! mutex))
719 (define (ir-tx-event-noerr-ack) (send-command-no-cr "n" #t))
720 (define (ir-tx-event-err-ack) (send-command-no-cr "e" #t))
722 (define (send-command-no-cr cmd trace?)
723 (and (rs232-send-no-cr cmd trace?)
726 (define (send-command cmd trace?)
727 (and (rs232-send cmd trace?)
730 (define (check-ok trace?)
731 (let ((answer (rs232-read-line trace?)))
732 (and (string? answer)
733 (= (string-length answer) 1)
734 (char=? (string-ref answer 0) #\!))))
736 (define (byte->string n)
737 (define (hex n) (string-ref "0123456789ABCDEF" (modulo n 16)))
738 (string (hex (quotient n 16))
741 (define (ir-tx-special byte1 byte2)
746 (list byte1 byte2)))))
747 (send-command-no-cr cmd #t)))
749 (define (ir-tx dest bytes)
754 (cons dest (u8vector->list bytes))))))
755 (send-command cmd #t)))
757 (define (poll-status)
758 (and (rs232-send-no-cr "p" #t)
759 (let ((answer (rs232-read-line #t)))
760 (and (string? answer)
761 (= (string-length answer) 3)
762 (char=? (string-ref answer 0) #\=)
763 (string->number (substring answer 1 3) 16)))))
766 (and (rs232-send-no-cr "r" #t)
767 (let ((answer (rs232-read-line #t)))
768 (and (string? answer)
769 (>= (string-length answer) 3)
770 (odd? (string-length answer))
771 (char=? (string-ref answer 0) #\=)
772 (let ((n (quotient (string-length answer) 2)))
773 (let ((v (make-u8vector n 0)))
774 (let loop ((i (- n 1)))
776 (let* ((j (+ (* i 2) 1))
778 (substring answer j (+ j 2))
782 (u8vector-set! v i x)
786 (define MSG_TYPE_ACK 0)
787 (define MSG_TYPE_SET_PROG_MODE 1)
788 (define MSG_TYPE_NORMAL 0)
789 (define MSG_TYPE_PROGRAM 1)
790 (define MSG_TYPE_STDIO 7)
792 (define NOERR_MASK 1)
795 (define CLOCK_MASK 8)
798 (define (rs232-flush-input)
799 (input-port-timeout-set! rs232 0)
800 (read-line rs232 #f))
802 (define no-response-count 0)
804 (define (rs232-read-line trace?)
805 (input-port-timeout-set! rs232 0.5)
806 (let ((x (read-line rs232)))
808 (if (and debug? trace?)
809 (pp (list '(rs232-read-line) '-> x)))
812 (set! no-response-count (+ no-response-count 1))
813 (if (> no-response-count 100)
815 (pp 'base-station-not-responding)
816 (set! no-response-count 50))))
818 (if (and debug? trace?)
823 (if (>= no-response-count 50)
824 (pp 'base-station-now-ok))
825 (set! no-response-count 0)))
828 (define (rs232-send-no-check str trace?)
830 (if (and debug? trace?)
831 (pp (list 'rs232-send-no-check str)))
835 (if (and debug? trace?)
841 (define (rs232-send-no-cr-no-check str trace?)
843 (if (and debug? trace?)
844 (pp (list 'rs232-send-no-cr-no-check str)))
847 (if (and debug? trace?)
853 (define (rs232-send str trace?)
854 (rs232-send-no-check str trace?)
855 (let ((echo (rs232-read-line #f)))
856 (if (and debug? trace? (string? echo))
862 (string=? echo str))))
864 (define (rs232-send-no-cr str trace?)
865 (rs232-send-no-cr-no-check str trace?)
866 (let ((echo (rs232-read-line trace?)))
868 (string=? echo str))))
870 (define (serve connection)
874 (let ((id-and-hostname (read connection)))
875 (if (and (pair? id-and-hostname)
876 (pair? (cdr id-and-hostname))
877 (null? (cddr id-and-hostname))
878 (exact-int? (car id-and-hostname))
879 (>= (car id-and-hostname) 0)
880 (< (car id-and-hostname) nb-ids))
881 (let ((id (car id-and-hostname))
882 (hostname (cadr id-and-hostname)))
884 (let ((client (vector-ref clients id)))
887 (mutex-unlock! mutex)
889 (list "============================================= connection to robot " id " from " hostname " **REFUSED**\n"))
893 (list "============================================= connection to robot " id " from " hostname " **REFUSED**\n")
895 (force-output log-file)))
896 (close-port connection))
898 (vector-set! clients id connection)
899 (mutex-unlock! mutex)
901 (list "============================================= connection to robot " id " from " hostname "\n"))
905 (list "============================================= connection to robot " id " from " hostname "\n")
907 (force-output log-file)))
908 (send '(ok) connection)
909 (process-client-commands connection id)
911 (vector-set! clients id #f)
912 (mutex-unlock! mutex)
913 (close-port connection)))))))))))
915 (define (process-client-commands connection id)
916 (with-exception-catcher
921 (let ((cmd (read connection)))
924 (send (vector id cmd) multiplexer)
927 ;------------------------------------------------------------------------------