| blob | 0a7c435cc8e8337ba83afb8eab4503355f1d2ec7 |
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]]
7 (define debug? #f)
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
21 (define serial-port-name "rs232") ; ADDED now to named pipe
23 (let loop ((lst '("/dev/cu.USA28X181P1.1"
24 "/dev/cu.USA28X181P2.2"
25 "/dev/cu.USA28X191P1.1"
26 "/dev/cu.USA28X191P2.2"
27 "/dev/ttyS0"
28 "/dev/ttyS1")))
29 (if (not (null? lst))
30 (let ((name (car lst)))
31 (if (file-exists? name)
32 (set! serial-port-name name)
33 (loop (cdr lst))))))
35 ;------------------------------------------------------------------------------
37 (define log-file
38 (and debug?
39 (with-exception-catcher
40 (lambda (exc)
41 #f)
42 (lambda ()
43 (open-output-file (list path: "robot.log" buffering: 'line))))))
45 ;------------------------------------------------------------------------------
47 (current-user-interrupt-handler exit)
49 (define (main . args)
51 (define (usage)
52 (display "usage: robot [[BASE_HOSTNAME|.] ID [HEX_FILE]]\n"))
54 (define (parse-arg1 args)
55 (if (null? args)
56 (parse-arg4 #f #f #f)
57 (let ((arg (car args)))
58 (if (exact-int? (string->number arg))
59 (parse-arg2 default-base args)
60 (parse-arg2 arg (cdr args))))))
62 (define (parse-arg2 base args)
63 (if (null? args)
64 (usage)
65 (let ((arg (string->number (car args))))
66 (if (and (exact-int? arg)
67 (>= arg 0)
68 (< arg nb-ids))
69 (parse-arg3 base arg (cdr args))
70 (usage)))))
72 (define (parse-arg3 base id args)
73 (if (null? args)
74 (parse-arg4 base id #f)
75 (let ((arg (car args)))
76 (if (null? (cdr args))
77 (parse-arg4 base id arg)
78 (usage)))))
80 (define (parse-arg4 base id filename)
81 (if id
82 (start-client base id filename program-start-addr)
83 (start-base)))
85 (parse-arg1 args))
87 (define (exact-int? x)
88 (and (integer? x) (exact? x)))
90 (define (start-base)
91 (multiplex)
92 (let ((connection-queue
93 (open-tcp-server
94 (list port-number: port-number
95 reuse-address: #t
96 eol-encoding: 'cr-lf))))
97 (let loop ()
98 (serve (read connection-queue))
99 (loop))))
101 (define (start-client base id filename start-addr)
102 (set! program-start-addr start-addr)
103 (let ((connection
104 (if (string=? base ".")
105 (receive (client server) (open-string-pipe)
106 (multiplex)
107 (serve server)
108 client)
109 (open-tcp-client
110 (list server-address: base
111 port-number: port-number
112 eol-encoding: 'cr-lf)))))
113 (send (list id
114 (with-exception-catcher
115 (lambda (exc)
116 "???")
117 (lambda ()
118 (host-info-name (host-info "")))))
119 connection)
120 (let ((ack (read connection)))
121 (if (equal? ack '(ok))
122 (begin
123 (if filename
124 (begin
125 (set! program-filename filename)
126 (start-client-upload connection id)))
127 (start-client-console connection id))
128 (display
129 (list "Another client is already connected to robot " id "\n"))))
130 (close-port connection)))
132 (define (start-client-upload connection id)
133 (let ((mem (read-hex-file program-filename)))
134 (if mem
135 (upload connection id mem program-start-addr))))
137 (define (start-client-console connection id)
139 (define (restart-robot)
140 (restart connection id)
141 (start-client-console connection id))
143 (define (upload-again)
144 (start-client-upload connection id)
145 (start-client-console connection id))
147 (define (stop-robot)
148 (stop connection id)
149 (start-client-console connection id))
151 (display
152 (list "###\n### Console:\n"))
153 (let ((input (repl-input-port)))
154 (if (tty? input)
155 (tty-mode-set! input #t #t #t #f 0)))
156 (let ((input (repl-input-port))
157 (can-send-key #t)
158 (tx-seq-num 0)
159 (rx-seq-num 0))
160 (let loop1 ((state 0))
161 (input-port-timeout-set! connection 0.01)
162 (let ((x (read connection)))
163 (if (not (eof-object? x))
164 (cond ((or (eq? x 'err)
165 (eq? x 'noerr))
166 (set! can-send-key #t))
167 (else
168 (if debug? (pp x))
169 (if (u8vector? x)
170 (if (and (>= (u8vector-length x) 3)
171 (= (quotient (u8vector-ref x 0) nb-ids)
172 MSG_TYPE_STDIO))
173 (let ((seq-num (u8vector-ref x 1)))
174 (if (not (= seq-num rx-seq-num))
175 (begin
176 (set! rx-seq-num seq-num)
177 (let loop2 ((i 2))
178 (if (< i (u8vector-length x))
179 (let ((n (u8vector-ref x i)))
180 (cond ((= n 10)
181 (display "\n"))
182 ;((= n 13)
183 ;(display "\n"))
184 ((or (< n 32) (> n 126))
185 (display
186 (list "<" n ">")))
187 (else
188 (write-char (integer->char n))))
189 (loop2 (+ i 1))))))))
190 (write (u8vector->list x))))))))
191 (if can-send-key
192 (begin
193 (input-port-timeout-set! input 0.01)
194 (let ((x (read-char input)))
196 (define (got x)
197 (send
198 (vector 'send-message
199 (+ id (* nb-ids MSG_TYPE_STDIO))
200 (u8vector tx-seq-num
201 (char->integer x)))
202 connection)
203 (set! can-send-key #f)
204 (set! tx-seq-num (modulo (+ tx-seq-num 1) 256))
205 (loop1 0))
207 (if (char? x)
208 (cond ((char=? x #\tab)
209 (upload-again))
210 (else
211 (cond ((= state 0)
212 (cond ((char=? x #\u001b)
213 (loop1 1))
214 (else
215 (got x))))
216 ((= state 1)
217 (cond ;((char=? x #\u001b))
218 ((char=? x #\[)
219 (loop1 3))
220 ((char=? x #\O)
221 (loop1 2))
222 (else
223 (got x))))
224 ((= state 2)
225 (cond ((char=? x #\P) ; F1
226 (stop-robot))
227 ((char=? x #\Q) ; F2
228 (restart-robot))
229 ((char=? x #\R) ; F3
230 (upload-again))
231 ((char=? x #\S) ; F4
232 )
233 (else
234 #f)))
235 (else
236 (cond ((char=? x #\A)
237 (got #\u008d))
238 ((char=? x #\B)
239 (got #\u008f))
240 ((char=? x #\C)
241 (got #\u008e))
242 ((char=? x #\D)
243 (got #\u008c))
244 (else
245 (got x)))))))
246 (loop1 state))))
247 (loop1 state)))))
249 ;------------------------------------------------------------------------------
251 (define (read-hex-file filename)
253 (define addr-width 32)
255 (define (syntax-error)
256 (error "Improper HEX file"))
258 (let ((f
259 (with-exception-catcher
260 (lambda (exc)
261 #f)
262 (lambda ()
263 (open-input-file filename)))))
265 (define mem (make-vector 16 #f))
267 (define (mem-store! a b)
268 (let loop ((m mem)
269 (a a)
270 (x (- addr-width 4)))
271 (if (= x 0)
272 (vector-set! m a b)
273 (let ((i (arithmetic-shift a (- x))))
274 (let ((v (vector-ref m i)))
275 (loop (or v
276 (let ((v (make-vector 16 #f)))
277 (vector-set! m i v)
278 v))
279 (- a (arithmetic-shift i x))
280 (- x 4)))))))
282 (define (mem->list)
284 (define (f m a n tail)
286 (define (g i a n tail)
287 (if (>= i 0)
288 (g (- i 1) (- a n) n (f (vector-ref m i) a n tail))
289 tail))
291 (if m
292 (if (= n 1)
293 (cons (cons (- a 1) m) tail)
294 (g 15 a (quotient n 16) tail))
295 tail))
297 (f mem (expt 2 addr-width) (expt 2 addr-width) '()))
299 (define hi16
300 0)
302 (define (read-hex-nibble)
303 (let ((c (read-char f)))
304 (cond ((and (char>=? c #\0) (char<=? c #\9))
305 (- (char->integer c) (char->integer #\0)))
306 ((and (char>=? c #\A) (char<=? c #\F))
307 (+ 10 (- (char->integer c) (char->integer #\A))))
308 ((and (char>=? c #\a) (char<=? c #\f))
309 (+ 10 (- (char->integer c) (char->integer #\a))))
310 (else
311 (syntax-error)))))
313 (define (read-hex-byte)
314 (let* ((a (read-hex-nibble))
315 (b (read-hex-nibble)))
316 (+ b (* a 16))))
318 (if f
319 (begin
320 (let loop1 ()
321 (let ((c (read-char f)))
322 (cond ((not (char? c)))
323 ((or (char=? c #\linefeed)
324 (char=? c #\return))
325 (loop1))
326 ((not (char=? c #\:))
327 (syntax-error))
328 (else
329 (let* ((len (read-hex-byte))
330 (a1 (read-hex-byte))
331 (a2 (read-hex-byte))
332 (type (read-hex-byte)))
333 (let* ((adr (+ a2 (* 256 a1)))
334 (sum (+ len a1 a2 type)))
335 (cond ((= type 0)
336 (let loop2 ((i 0))
337 (if (< i len)
338 (let ((a (+ adr (* hi16 65536)))
339 (b (read-hex-byte)))
340 (mem-store! a b)
341 (set! adr (modulo (+ adr 1) 65536))
342 (set! sum (+ sum b))
343 (loop2 (+ i 1))))))
344 ((= type 1)
345 (if (not (= len 0))
346 (syntax-error)))
347 ((= type 4)
348 (if (not (= len 2))
349 (syntax-error))
350 (let* ((a1 (read-hex-byte))
351 (a2 (read-hex-byte)))
352 (set! sum (+ sum a1 a2))
353 (set! hi16 (+ a2 (* 256 a1)))))
354 (else
355 (syntax-error)))
356 (let ((check (read-hex-byte)))
357 (if (not (= (modulo (- sum) 256) check))
358 (syntax-error)))
359 (let ((c (read-char f)))
360 (if (or (not (or (char=? c #\linefeed)
361 (char=? c #\return)))
362 (not (= type 1)))
363 (loop1)))))))))
365 (close-input-port f)
367 (mem->list))
368 (begin
369 (display
370 (list "\n### The file " filename " does not exist\n"))
371 #f))))
373 (define (upload connection id mem start-addr)
375 (define max-programmable-address 65535)
377 (define bp 8) ; program block size
378 (define be 64) ; erase block size
380 (if (start-programming connection id)
381 (begin
382 (let loop1 ((last-erased-be -1)
383 (lst mem))
384 (if (pair? lst)
385 (let* ((x (car lst))
386 (a (car x))
387 (a-bp (quotient a bp))
388 (a-be (quotient a be))
389 (bp-bytes (make-u8vector bp 255)))
390 (if (<= a max-programmable-address)
391 (if (or (= a-be last-erased-be)
392 (let ((a (* a-be be)))
393 (or (< a start-addr)
394 (erase-block connection id a))))
395 (begin
396 (u8vector-set! bp-bytes (modulo a bp) (cdr x))
397 (let loop2 ((lst2 (cdr lst)))
398 (if (and (pair? lst2)
399 (let ((a (car (car lst2))))
400 (and (<= a max-programmable-address)
401 (= (quotient a bp) a-bp))))
402 (begin
403 (u8vector-set! bp-bytes
404 (modulo (car (car lst2)) bp)
405 (cdr (car lst2)))
406 (loop2 (cdr lst2)))
407 (if (let ((a (* a-bp bp)))
408 (or (< a start-addr)
409 (program-block connection id a bp-bytes)))
410 (loop1 a-be
411 lst2))))))
412 (reboot connection id)))
413 (reboot connection id))))))
415 (define (request cmd connection)
416 (let loop ((n 10))
417 (if (> n 0)
418 (begin
419 (display ".")
420 (let ((x (request-once cmd connection)))
421 (if (eq? x 'err)
422 (begin
423 (thread-sleep! 2)
424 (loop (- n 1)))
425 (begin
426 (display "\n")
427 #t))))
428 (begin
429 (display " ERROR!\n")
430 #f))))
432 (define (request-once cmd connection)
433 (send cmd connection)
434 (let loop ()
435 (let ((x (read connection)))
436 (cond ((or (eq? x 'err)
437 (eq? x 'noerr))
438 x)
439 (else
440 (loop))))))
442 (define (request-version connection id)
444 (define (version-msg? version)
445 (and (u8vector? version)
446 (= (u8vector-length version) 5)
447 (= (u8vector-ref version 1)
448 (quotient version-addr 256))
449 (= (u8vector-ref version 2)
450 (modulo version-addr 256))))
452 (define (return x)
453 (input-port-timeout-set! connection #f)
454 x)
456 (request-once (vector 'set-program-mode id) connection)
457 (send
458 (vector 'send-message
459 (+ id (* nb-ids MSG_TYPE_PROGRAM))
460 (u8vector (quotient version-addr 256)
461 (modulo version-addr 256)
462 1))
463 connection)
464 (input-port-timeout-set! connection 1)
465 (let loop ((ack #f)
466 (version #f))
467 (let ((x (read connection)))
468 (cond ((eof-object? x)
469 (if ack
470 (return #f)
471 (loop ack version)))
472 ((or (eq? x 'err)
473 (eq? x 'noerr))
474 (if version
475 (return version)
476 (loop #t #f)))
477 (else
478 (if (version-msg? x)
479 (if ack
480 (return x)
481 (loop #f x))
482 (loop ack version)))))))
484 (define (send obj port)
485 (write obj port)
486 (newline port)
487 (force-output port))
489 (define (start-programming connection id)
490 (display
491 (list "\n### Programming robot " id " with " program-filename))
492 (enter-program-mode connection id))
494 (define (stop connection id)
495 (display
496 (list "\n### Stopping robot " id))
497 (enter-program-mode connection id))
499 (define (restart connection id)
500 (display
501 (list "###\n### Connecting to robot " id))
502 (enter-program-mode connection id)
503 (reboot connection id))
505 (define (enter-program-mode connection id)
506 (let loop ((n 5))
507 (if (> n 0)
508 (begin
509 (display ".")
510 (let ((version (request-version connection id)))
511 (if version
512 (let ((version-major (u8vector-ref version 3))
513 (version-minor (u8vector-ref version 4)))
514 (if (and (= version-major 1)
515 (= version-minor 0))
516 (begin
517 (display "\n")
518 #t)
519 (begin
520 (display " INCOMPATIBLE FIRMWARE!\n")
521 #f)))
522 (loop (- n 1)))))
523 (begin
524 (display " THE ROBOT IS NOT RESPONDING!\n")
525 #f))))
527 (define (erase-block connection id addr)
528 ; (set! addr (+ addr #x2000))
529 (display
530 (list "###\n### Erasing block 0x"
531 (number->string addr 16)))
532 (request
533 (vector 'send-message
534 (+ id (* nb-ids MSG_TYPE_PROGRAM))
535 (u8vector (quotient addr 256)
536 (modulo addr 256)))
537 connection))
539 (define (program-block connection id addr bytes)
540 ; (set! addr (+ addr #x2000))
541 (display
542 (list "### Programming block 0x"
543 (number->string addr 16)))
544 (request
545 (vector 'send-message
546 (+ id (* nb-ids MSG_TYPE_PROGRAM))
547 (u8vector-append
548 (u8vector (quotient addr 256)
549 (modulo addr 256))
550 bytes))
551 connection))
553 (define (reboot connection id)
554 (display
555 (list "###\n### Restarting robot"))
556 (request
557 (vector 'send-message
558 (+ id (* nb-ids MSG_TYPE_PROGRAM))
559 (u8vector 0 0))
560 connection))
562 ;------------------------------------------------------------------------------
564 ; Server side.
566 (define nb-ids 32)
567 (define mutex #f)
568 (define clients #f)
569 (define multiplexer #f)
570 (define rs232 #f)
572 (define (multiplex)
574 (set! mutex (make-mutex))
575 (set! clients (make-vector nb-ids #f))
576 (set! multiplexer (open-vector))
578 (set! rs232
579 (open-file
580 (list path: serial-port-name
581 eol-encoding: 'cr-lf)))
582 (if (tty? rs232)
583 (tty-mode-set! rs232 #f #f #t #t 38400))
585 (thread-sleep! 0.1)
586 (rs232-flush-input)
588 (thread-start!
589 (make-thread
590 (lambda ()
591 (let loop1 ()
592 (input-port-timeout-set! multiplexer 0.01)
593 (let loop2 ()
594 (let ((x (read multiplexer)))
595 (if (vector? x)
596 (let* ((id (vector-ref x 0))
597 (cmd (vector-ref x 1))
598 (cmd-type (vector-ref cmd 0)))
599 (cond ((eq? cmd-type 'send-message)
600 (let ((dest (vector-ref cmd 1))
601 (bytes (vector-ref cmd 2)))
602 (if (send-message dest bytes)
603 (let ((s (wait-until-end-of-tx)))
604 (cond ((not s)
605 (send-to-id 'err id)
606 (loop2))
607 ((not (= (bitwise-and s NOERR_MASK) 0))
608 (ir-tx-event-noerr-ack)
609 (send-to-id 'noerr id)
610 (loop2))
611 ((not (= (bitwise-and s ERR_MASK) 0))
612 (ir-tx-event-err-ack)
613 (send-to-id 'err id)
614 (loop2))
615 (else
616 (send-to-id 'err id)
617 (loop2))))
618 (begin
619 (send-to-id 'err id)
620 (loop2)))))
621 ((eq? cmd-type 'set-program-mode)
622 (let ((dest (vector-ref cmd 1)))
623 (if (set-program-mode dest)
624 (let ((s (wait-until-end-of-tx)))
625 (cond ((not s)
626 (send-to-id 'err id)
627 (loop2))
628 ((not (= (bitwise-and s NOERR_MASK) 0))
629 (ir-tx-event-noerr-ack)
630 (send-to-id 'noerr id)
631 (loop2))
632 ((not (= (bitwise-and s ERR_MASK) 0))
633 (ir-tx-event-err-ack)
634 (send-to-id 'noerr id)
635 (loop2))
636 (else
637 (send-to-id 'err id)
638 (loop2))))
639 (begin
640 (send-to-id 'err id)
641 (loop2)))))
642 (else
643 (loop2))))
644 (begin
645 (poll-status-handling-rx)
646 (loop1))))))))))
648 (define (set-program-mode dest)
649 (let ((s (prepare-to-tx)))
650 (and s
651 (let ((b (+ dest (* nb-ids MSG_TYPE_SET_PROG_MODE))))
652 (ir-tx-special (- #xff b) b)))))
654 (define (send-message dest bytes)
655 (let ((s (prepare-to-tx)))
656 (and s
657 (begin
658 (display
659 (list "sending to " (modulo dest nb-ids) ": "))
660 (write (u8vector->list bytes))
661 (display "\n")
662 (ir-tx dest bytes)))))
664 (define (prepare-to-tx)
665 (let loop ()
666 (let ((s (wait-until-end-of-tx)))
667 (cond ((not s)
668 #f)
669 ((not (= (bitwise-and s NOERR_MASK) 0))
670 (ir-tx-event-noerr-ack)
671 (loop))
672 ((not (= (bitwise-and s ERR_MASK) 0))
673 (ir-tx-event-err-ack)
674 (loop))
675 (else
676 s)))))
678 (define (wait-until-end-of-tx)
679 (let loop ()
680 (let ((s (poll-status-handling-rx)))
681 (cond ((not s)
682 #f)
683 ((not (= (bitwise-and s TX_MASK) 0))
684 (loop))
685 (else
686 s)))))
688 (define (poll-status-handling-rx)
689 (let loop ()
690 (let ((s (poll-status)))
691 (cond ((not s)
692 #f)
693 ((not (= (bitwise-and s RX_MASK) 0))
694 (handle-rx-message)
695 (loop))
696 (else
697 s)))))
699 (define (handle-rx-message)
700 (let ((msg (ir-rx)))
701 (if msg
702 (let ((id (modulo (u8vector-ref msg 0) nb-ids)))
703 (display
704 (list " received from " id ": "))
705 (write (u8vector->list msg))
706 (display "\n")
707 (send-to-id msg id)))))
709 (define (send-to-id msg id)
710 (mutex-lock! mutex)
711 (let ((client (vector-ref clients id)))
712 (if client
713 (with-exception-catcher
714 (lambda (exc)
715 (vector-set! clients id #f))
716 (lambda ()
717 (send msg client)))))
718 (mutex-unlock! mutex))
720 (define (ir-tx-event-noerr-ack) (send-command-no-cr "n" #t))
721 (define (ir-tx-event-err-ack) (send-command-no-cr "e" #t))
723 (define (send-command-no-cr cmd trace?)
724 (and (rs232-send-no-cr cmd trace?)
725 (check-ok trace?)))
727 (define (send-command cmd trace?)
728 (and (rs232-send cmd trace?)
729 (check-ok trace?)))
731 (define (check-ok trace?)
732 (let ((answer (rs232-read-line trace?)))
733 (and (string? answer)
734 (= (string-length answer) 1)
735 (char=? (string-ref answer 0) #\!))))
737 (define (byte->string n)
738 (define (hex n) (string-ref "0123456789ABCDEF" (modulo n 16)))
739 (string (hex (quotient n 16))
740 (hex n)))
742 (define (ir-tx-special byte1 byte2)
743 (let ((cmd
744 (apply string-append
745 "s"
746 (map byte->string
747 (list byte1 byte2)))))
748 (send-command-no-cr cmd #t)))
750 (define (ir-tx dest bytes)
751 (let ((cmd
752 (apply string-append
753 "t"
754 (map byte->string
755 (cons dest (u8vector->list bytes))))))
756 (send-command cmd #t)))
758 (define (poll-status)
759 (and (rs232-send-no-cr "p" #t)
760 (let ((answer (rs232-read-line #t)))
761 (and (string? answer)
762 (= (string-length answer) 3)
763 (char=? (string-ref answer 0) #\=)
764 (string->number (substring answer 1 3) 16)))))
766 (define (ir-rx)
767 (and (rs232-send-no-cr "r" #t)
768 (let ((answer (rs232-read-line #t)))
769 (and (string? answer)
770 (>= (string-length answer) 3)
771 (odd? (string-length answer))
772 (char=? (string-ref answer 0) #\=)
773 (let ((n (quotient (string-length answer) 2)))
774 (let ((v (make-u8vector n 0)))
775 (let loop ((i (- n 1)))
776 (if (>= i 0)
777 (let* ((j (+ (* i 2) 1))
778 (x (string->number
779 (substring answer j (+ j 2))
780 16)))
781 (and x
782 (begin
783 (u8vector-set! v i x)
784 (loop (- i 1)))))
785 v))))))))
787 (define MSG_TYPE_ACK 0)
788 (define MSG_TYPE_SET_PROG_MODE 1)
789 (define MSG_TYPE_NORMAL 0)
790 (define MSG_TYPE_PROGRAM 1)
791 (define MSG_TYPE_STDIO 7)
793 (define NOERR_MASK 1)
794 (define ERR_MASK 2)
795 (define RX_MASK 4)
796 (define CLOCK_MASK 8)
797 (define TX_MASK 128)
799 (define (rs232-flush-input)
800 (input-port-timeout-set! rs232 0)
801 (read-line rs232 #f))
803 (define no-response-count 0)
805 (define (rs232-read-line trace?)
806 (input-port-timeout-set! rs232 0.5)
807 (let ((x (read-line rs232)))
808 '
809 (if (and debug? trace?)
810 (pp (list '(rs232-read-line) '-> x)))
811 (if (eof-object? x)
812 (begin
813 (set! no-response-count (+ no-response-count 1))
814 (if (> no-response-count 100)
815 (begin
816 (pp 'base-station-not-responding)
817 (set! no-response-count 50))))
818 (begin
819 (if (and debug? trace?)
820 (begin
821 (display "<- ")
822 (display x)
823 (display "\n")))
824 (if (>= no-response-count 50)
825 (pp 'base-station-now-ok))
826 (set! no-response-count 0)))
827 x))
829 (define (rs232-send-no-check str trace?)
830 '
831 (if (and debug? trace?)
832 (pp (list 'rs232-send-no-check str)))
833 (display str rs232)
834 (display "\r" rs232)
835 (force-output rs232)
836 (if (and debug? trace?)
837 (begin
838 (display "-> ")
839 (display str)
840 (display "\n"))))
842 (define (rs232-send-no-cr-no-check str trace?)
843 '
844 (if (and debug? trace?)
845 (pp (list 'rs232-send-no-cr-no-check str)))
846 (display str rs232)
847 (force-output rs232)
848 (if (and debug? trace?)
849 (begin
850 (display "-> ")
851 (display str)
852 (display "\n"))))
854 (define (rs232-send str trace?)
855 (rs232-send-no-check str trace?)
856 (let ((echo (rs232-read-line #f)))
857 (if (and debug? trace? (string? echo))
858 (begin
859 (display "<- ")
860 (display echo)
861 (display "\n")))
862 (and (string? echo)
863 (string=? echo str))))
865 (define (rs232-send-no-cr str trace?)
866 (rs232-send-no-cr-no-check str trace?)
867 (let ((echo (rs232-read-line trace?)))
868 (and (string? echo)
869 (string=? echo str))))
871 (define (serve connection)
872 (thread-start!
873 (make-thread
874 (lambda ()
875 (let ((id-and-hostname (read connection)))
876 (if (and (pair? id-and-hostname)
877 (pair? (cdr id-and-hostname))
878 (null? (cddr id-and-hostname))
879 (exact-int? (car id-and-hostname))
880 (>= (car id-and-hostname) 0)
881 (< (car id-and-hostname) nb-ids))
882 (let ((id (car id-and-hostname))
883 (hostname (cadr id-and-hostname)))
884 (mutex-lock! mutex)
885 (let ((client (vector-ref clients id)))
886 (if client
887 (begin
888 (mutex-unlock! mutex)
889 (display
890 (list "============================================= connection to robot " id " from " hostname " **REFUSED**\n"))
891 (if log-file
892 (begin
893 (display
894 (list "============================================= connection to robot " id " from " hostname " **REFUSED**\n")
895 log-file)
896 (force-output log-file)))
897 (close-port connection))
898 (begin
899 (vector-set! clients id connection)
900 (mutex-unlock! mutex)
901 (display
902 (list "============================================= connection to robot " id " from " hostname "\n"))
903 (if log-file
904 (begin
905 (display
906 (list "============================================= connection to robot " id " from " hostname "\n")
907 log-file)
908 (force-output log-file)))
909 (send '(ok) connection)
910 (process-client-commands connection id)
911 (mutex-lock! mutex)
912 (vector-set! clients id #f)
913 (mutex-unlock! mutex)
914 (close-port connection)))))))))))
916 (define (process-client-commands connection id)
917 (with-exception-catcher
918 (lambda (exc)
919 #f)
920 (lambda ()
921 (let loop ()
922 (let ((cmd (read connection)))
923 (if (vector? cmd)
924 (begin
925 (send (vector id cmd) multiplexer)
926 (loop))))))))
928 ;------------------------------------------------------------------------------