| blob | 4fedae002c00fe8acccf7dfc1e66be7b171f0876 |
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
22 (let loop ((lst '("/dev/cu.USA28X181P1.1"
23 "/dev/cu.USA28X181P2.2"
24 "/dev/cu.USA28X191P1.1"
25 "/dev/cu.USA28X191P2.2"
26 "/dev/ttyS0"
27 "/dev/ttyS1")))
28 (if (not (null? lst))
29 (let ((name (car lst)))
30 (if (file-exists? name)
31 (set! serial-port-name name)
32 (loop (cdr lst))))))
34 ;------------------------------------------------------------------------------
36 (define log-file
37 (and debug?
38 (with-exception-catcher
39 (lambda (exc)
40 #f)
41 (lambda ()
42 (open-output-file (list path: "robot.log" buffering: 'line))))))
44 ;------------------------------------------------------------------------------
46 (current-user-interrupt-handler exit)
48 (define (main . args)
50 (define (usage)
51 (display "usage: robot [[BASE_HOSTNAME|.] ID [HEX_FILE]]\n"))
53 (define (parse-arg1 args)
54 (if (null? args)
55 (parse-arg4 #f #f #f)
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)
62 (if (null? args)
63 (usage)
64 (let ((arg (string->number (car args))))
65 (if (and (exact-int? arg)
66 (>= arg 0)
67 (< arg nb-ids))
68 (parse-arg3 base arg (cdr args))
69 (usage)))))
71 (define (parse-arg3 base id args)
72 (if (null? args)
73 (parse-arg4 base id #f)
74 (let ((arg (car args)))
75 (if (null? (cdr args))
76 (parse-arg4 base id arg)
77 (usage)))))
79 (define (parse-arg4 base id filename)
80 (if id
81 (start-client base id filename program-start-addr)
82 (start-base)))
84 (parse-arg1 args))
86 (define (exact-int? x)
87 (and (integer? x) (exact? x)))
89 (define (start-base)
90 (multiplex)
91 (let ((connection-queue
92 (open-tcp-server
93 (list port-number: port-number
94 reuse-address: #t
95 eol-encoding: 'cr-lf))))
96 (let loop ()
97 (serve (read connection-queue))
98 (loop))))
100 (define (start-client base id filename start-addr)
101 (set! program-start-addr start-addr)
102 (let ((connection
103 (if (string=? base ".")
104 (receive (client server) (open-string-pipe)
105 (multiplex)
106 (serve server)
107 client)
108 (open-tcp-client
109 (list server-address: base
110 port-number: port-number
111 eol-encoding: 'cr-lf)))))
112 (send (list id
113 (with-exception-catcher
114 (lambda (exc)
115 "???")
116 (lambda ()
117 (host-info-name (host-info "")))))
118 connection)
119 (let ((ack (read connection)))
120 (if (equal? ack '(ok))
121 (begin
122 (if filename
123 (begin
124 (set! program-filename filename)
125 (start-client-upload connection id)))
126 (start-client-console connection id))
127 (display
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)))
133 (if mem
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))
146 (define (stop-robot)
147 (stop connection id)
148 (start-client-console connection id))
150 (display
151 (list "###\n### Console:\n"))
152 (let ((input (repl-input-port)))
153 (if (tty? input)
154 (tty-mode-set! input #t #t #t #f 0)))
155 (let ((input (repl-input-port))
156 (can-send-key #t)
157 (tx-seq-num 0)
158 (rx-seq-num 0))
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)
164 (eq? x 'noerr))
165 (set! can-send-key #t))
166 (else
167 (if debug? (pp x))
168 (if (u8vector? x)
169 (if (and (>= (u8vector-length x) 3)
170 (= (quotient (u8vector-ref x 0) nb-ids)
171 MSG_TYPE_STDIO))
172 (let ((seq-num (u8vector-ref x 1)))
173 (if (not (= seq-num rx-seq-num))
174 (begin
175 (set! rx-seq-num seq-num)
176 (let loop2 ((i 2))
177 (if (< i (u8vector-length x))
178 (let ((n (u8vector-ref x i)))
179 (cond ((= n 10)
180 (display "\n"))
181 ;((= n 13)
182 ;(display "\n"))
183 ((or (< n 32) (> n 126))
184 (display
185 (list "<" n ">")))
186 (else
187 (write-char (integer->char n))))
188 (loop2 (+ i 1))))))))
189 (write (u8vector->list x))))))))
190 (if can-send-key
191 (begin
192 (input-port-timeout-set! input 0.01)
193 (let ((x (read-char input)))
195 (define (got x)
196 (send
197 (vector 'send-message
198 (+ id (* nb-ids MSG_TYPE_STDIO))
199 (u8vector tx-seq-num
200 (char->integer x)))
201 connection)
202 (set! can-send-key #f)
203 (set! tx-seq-num (modulo (+ tx-seq-num 1) 256))
204 (loop1 0))
206 (if (char? x)
207 (cond ((char=? x #\tab)
208 (upload-again))
209 (else
210 (cond ((= state 0)
211 (cond ((char=? x #\u001b)
212 (loop1 1))
213 (else
214 (got x))))
215 ((= state 1)
216 (cond ;((char=? x #\u001b))
217 ((char=? x #\[)
218 (loop1 3))
219 ((char=? x #\O)
220 (loop1 2))
221 (else
222 (got x))))
223 ((= state 2)
224 (cond ((char=? x #\P) ; F1
225 (stop-robot))
226 ((char=? x #\Q) ; F2
227 (restart-robot))
228 ((char=? x #\R) ; F3
229 (upload-again))
230 ((char=? x #\S) ; F4
231 )
232 (else
233 #f)))
234 (else
235 (cond ((char=? x #\A)
236 (got #\u008d))
237 ((char=? x #\B)
238 (got #\u008f))
239 ((char=? x #\C)
240 (got #\u008e))
241 ((char=? x #\D)
242 (got #\u008c))
243 (else
244 (got x)))))))
245 (loop1 state))))
246 (loop1 state)))))
248 ;------------------------------------------------------------------------------
250 (define (read-hex-file filename)
252 (define addr-width 32)
254 (define (syntax-error)
255 (error "Improper HEX file"))
257 (let ((f
258 (with-exception-catcher
259 (lambda (exc)
260 #f)
261 (lambda ()
262 (open-input-file filename)))))
264 (define mem (make-vector 16 #f))
266 (define (mem-store! a b)
267 (let loop ((m mem)
268 (a a)
269 (x (- addr-width 4)))
270 (if (= x 0)
271 (vector-set! m a b)
272 (let ((i (arithmetic-shift a (- x))))
273 (let ((v (vector-ref m i)))
274 (loop (or v
275 (let ((v (make-vector 16 #f)))
276 (vector-set! m i v)
277 v))
278 (- a (arithmetic-shift i x))
279 (- x 4)))))))
281 (define (mem->list)
283 (define (f m a n tail)
285 (define (g i a n tail)
286 (if (>= i 0)
287 (g (- i 1) (- a n) n (f (vector-ref m i) a n tail))
288 tail))
290 (if m
291 (if (= n 1)
292 (cons (cons (- a 1) m) tail)
293 (g 15 a (quotient n 16) tail))
294 tail))
296 (f mem (expt 2 addr-width) (expt 2 addr-width) '()))
298 (define hi16
299 0)
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))))
309 (else
310 (syntax-error)))))
312 (define (read-hex-byte)
313 (let* ((a (read-hex-nibble))
314 (b (read-hex-nibble)))
315 (+ b (* a 16))))
317 (if f
318 (begin
319 (let loop1 ()
320 (let ((c (read-char f)))
321 (cond ((not (char? c)))
322 ((or (char=? c #\linefeed)
323 (char=? c #\return))
324 (loop1))
325 ((not (char=? c #\:))
326 (syntax-error))
327 (else
328 (let* ((len (read-hex-byte))
329 (a1 (read-hex-byte))
330 (a2 (read-hex-byte))
331 (type (read-hex-byte)))
332 (let* ((adr (+ a2 (* 256 a1)))
333 (sum (+ len a1 a2 type)))
334 (cond ((= type 0)
335 (let loop2 ((i 0))
336 (if (< i len)
337 (let ((a (+ adr (* hi16 65536)))
338 (b (read-hex-byte)))
339 (mem-store! a b)
340 (set! adr (modulo (+ adr 1) 65536))
341 (set! sum (+ sum b))
342 (loop2 (+ i 1))))))
343 ((= type 1)
344 (if (not (= len 0))
345 (syntax-error)))
346 ((= type 4)
347 (if (not (= len 2))
348 (syntax-error))
349 (let* ((a1 (read-hex-byte))
350 (a2 (read-hex-byte)))
351 (set! sum (+ sum a1 a2))
352 (set! hi16 (+ a2 (* 256 a1)))))
353 (else
354 (syntax-error)))
355 (let ((check (read-hex-byte)))
356 (if (not (= (modulo (- sum) 256) check))
357 (syntax-error)))
358 (let ((c (read-char f)))
359 (if (or (not (or (char=? c #\linefeed)
360 (char=? c #\return)))
361 (not (= type 1)))
362 (loop1)))))))))
364 (close-input-port f)
366 (mem->list))
367 (begin
368 (display
369 (list "\n### The file " filename " does not exist\n"))
370 #f))))
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)
380 (begin
381 (let loop1 ((last-erased-be -1)
382 (lst mem))
383 (if (pair? lst)
384 (let* ((x (car lst))
385 (a (car x))
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)))
392 (or (< a start-addr)
393 (erase-block connection id a))))
394 (begin
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))))
401 (begin
402 (u8vector-set! bp-bytes
403 (modulo (car (car lst2)) bp)
404 (cdr (car lst2)))
405 (loop2 (cdr lst2)))
406 (if (let ((a (* a-bp bp)))
407 (or (< a start-addr)
408 (program-block connection id a bp-bytes)))
409 (loop1 a-be
410 lst2))))))
411 (reboot connection id)))
412 (reboot connection id))))))
414 (define (request cmd connection)
415 (let loop ((n 10))
416 (if (> n 0)
417 (begin
418 (display ".")
419 (let ((x (request-once cmd connection)))
420 (if (eq? x 'err)
421 (begin
422 (thread-sleep! 2)
423 (loop (- n 1)))
424 (begin
425 (display "\n")
426 #t))))
427 (begin
428 (display " ERROR!\n")
429 #f))))
431 (define (request-once cmd connection)
432 (send cmd connection)
433 (let loop ()
434 (let ((x (read connection)))
435 (cond ((or (eq? x 'err)
436 (eq? x 'noerr))
437 x)
438 (else
439 (loop))))))
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))))
451 (define (return x)
452 (input-port-timeout-set! connection #f)
453 x)
455 (request-once (vector 'set-program-mode id) connection)
456 (send
457 (vector 'send-message
458 (+ id (* nb-ids MSG_TYPE_PROGRAM))
459 (u8vector (quotient version-addr 256)
460 (modulo version-addr 256)
461 1))
462 connection)
463 (input-port-timeout-set! connection 1)
464 (let loop ((ack #f)
465 (version #f))
466 (let ((x (read connection)))
467 (cond ((eof-object? x)
468 (if ack
469 (return #f)
470 (loop ack version)))
471 ((or (eq? x 'err)
472 (eq? x 'noerr))
473 (if version
474 (return version)
475 (loop #t #f)))
476 (else
477 (if (version-msg? x)
478 (if ack
479 (return x)
480 (loop #f x))
481 (loop ack version)))))))
483 (define (send obj port)
484 (write obj port)
485 (newline port)
486 (force-output port))
488 (define (start-programming connection id)
489 (display
490 (list "\n### Programming robot " id " with " program-filename))
491 (enter-program-mode connection id))
493 (define (stop connection id)
494 (display
495 (list "\n### Stopping robot " id))
496 (enter-program-mode connection id))
498 (define (restart connection id)
499 (display
500 (list "###\n### Connecting to robot " id))
501 (enter-program-mode connection id)
502 (reboot connection id))
504 (define (enter-program-mode connection id)
505 (let loop ((n 5))
506 (if (> n 0)
507 (begin
508 (display ".")
509 (let ((version (request-version connection id)))
510 (if version
511 (let ((version-major (u8vector-ref version 3))
512 (version-minor (u8vector-ref version 4)))
513 (if (and (= version-major 1)
514 (= version-minor 0))
515 (begin
516 (display "\n")
517 #t)
518 (begin
519 (display " INCOMPATIBLE FIRMWARE!\n")
520 #f)))
521 (loop (- n 1)))))
522 (begin
523 (display " THE ROBOT IS NOT RESPONDING!\n")
524 #f))))
526 (define (erase-block connection id addr)
527 ; (set! addr (+ addr #x2000))
528 (display
529 (list "###\n### Erasing block 0x"
530 (number->string addr 16)))
531 (request
532 (vector 'send-message
533 (+ id (* nb-ids MSG_TYPE_PROGRAM))
534 (u8vector (quotient addr 256)
535 (modulo addr 256)))
536 connection))
538 (define (program-block connection id addr bytes)
539 ; (set! addr (+ addr #x2000))
540 (display
541 (list "### Programming block 0x"
542 (number->string addr 16)))
543 (request
544 (vector 'send-message
545 (+ id (* nb-ids MSG_TYPE_PROGRAM))
546 (u8vector-append
547 (u8vector (quotient addr 256)
548 (modulo addr 256))
549 bytes))
550 connection))
552 (define (reboot connection id)
553 (display
554 (list "###\n### Restarting robot"))
555 (request
556 (vector 'send-message
557 (+ id (* nb-ids MSG_TYPE_PROGRAM))
558 (u8vector 0 0))
559 connection))
561 ;------------------------------------------------------------------------------
563 ; Server side.
565 (define nb-ids 32)
566 (define mutex #f)
567 (define clients #f)
568 (define multiplexer #f)
569 (define rs232 #f)
571 (define (multiplex)
573 (set! mutex (make-mutex))
574 (set! clients (make-vector nb-ids #f))
575 (set! multiplexer (open-vector))
577 (set! rs232
578 (open-file
579 (list path: serial-port-name
580 eol-encoding: 'cr-lf)))
581 (if (tty? rs232)
582 (tty-mode-set! rs232 #f #f #t #t 38400))
584 (thread-sleep! 0.1)
585 (rs232-flush-input)
587 (thread-start!
588 (make-thread
589 (lambda ()
590 (let loop1 ()
591 (input-port-timeout-set! multiplexer 0.01)
592 (let loop2 ()
593 (let ((x (read multiplexer)))
594 (if (vector? x)
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)))
603 (cond ((not s)
604 (send-to-id 'err id)
605 (loop2))
606 ((not (= (bitwise-and s NOERR_MASK) 0))
607 (ir-tx-event-noerr-ack)
608 (send-to-id 'noerr id)
609 (loop2))
610 ((not (= (bitwise-and s ERR_MASK) 0))
611 (ir-tx-event-err-ack)
612 (send-to-id 'err id)
613 (loop2))
614 (else
615 (send-to-id 'err id)
616 (loop2))))
617 (begin
618 (send-to-id 'err id)
619 (loop2)))))
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)))
624 (cond ((not s)
625 (send-to-id 'err id)
626 (loop2))
627 ((not (= (bitwise-and s NOERR_MASK) 0))
628 (ir-tx-event-noerr-ack)
629 (send-to-id 'noerr id)
630 (loop2))
631 ((not (= (bitwise-and s ERR_MASK) 0))
632 (ir-tx-event-err-ack)
633 (send-to-id 'noerr id)
634 (loop2))
635 (else
636 (send-to-id 'err id)
637 (loop2))))
638 (begin
639 (send-to-id 'err id)
640 (loop2)))))
641 (else
642 (loop2))))
643 (begin
644 (poll-status-handling-rx)
645 (loop1))))))))))
647 (define (set-program-mode dest)
648 (let ((s (prepare-to-tx)))
649 (and s
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)))
655 (and s
656 (begin
657 (display
658 (list "sending to " (modulo dest nb-ids) ": "))
659 (write (u8vector->list bytes))
660 (display "\n")
661 (ir-tx dest bytes)))))
663 (define (prepare-to-tx)
664 (let loop ()
665 (let ((s (wait-until-end-of-tx)))
666 (cond ((not s)
667 #f)
668 ((not (= (bitwise-and s NOERR_MASK) 0))
669 (ir-tx-event-noerr-ack)
670 (loop))
671 ((not (= (bitwise-and s ERR_MASK) 0))
672 (ir-tx-event-err-ack)
673 (loop))
674 (else
675 s)))))
677 (define (wait-until-end-of-tx)
678 (let loop ()
679 (let ((s (poll-status-handling-rx)))
680 (cond ((not s)
681 #f)
682 ((not (= (bitwise-and s TX_MASK) 0))
683 (loop))
684 (else
685 s)))))
687 (define (poll-status-handling-rx)
688 (let loop ()
689 (let ((s (poll-status)))
690 (cond ((not s)
691 #f)
692 ((not (= (bitwise-and s RX_MASK) 0))
693 (handle-rx-message)
694 (loop))
695 (else
696 s)))))
698 (define (handle-rx-message)
699 (let ((msg (ir-rx)))
700 (if msg
701 (let ((id (modulo (u8vector-ref msg 0) nb-ids)))
702 (display
703 (list " received from " id ": "))
704 (write (u8vector->list msg))
705 (display "\n")
706 (send-to-id msg id)))))
708 (define (send-to-id msg id)
709 (mutex-lock! mutex)
710 (let ((client (vector-ref clients id)))
711 (if client
712 (with-exception-catcher
713 (lambda (exc)
714 (vector-set! clients id #f))
715 (lambda ()
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?)
724 (check-ok trace?)))
726 (define (send-command cmd trace?)
727 (and (rs232-send cmd trace?)
728 (check-ok 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))
739 (hex n)))
741 (define (ir-tx-special byte1 byte2)
742 (let ((cmd
743 (apply string-append
744 "s"
745 (map byte->string
746 (list byte1 byte2)))))
747 (send-command-no-cr cmd #t)))
749 (define (ir-tx dest bytes)
750 (let ((cmd
751 (apply string-append
752 "t"
753 (map byte->string
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)))))
765 (define (ir-rx)
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)))
775 (if (>= i 0)
776 (let* ((j (+ (* i 2) 1))
777 (x (string->number
778 (substring answer j (+ j 2))
779 16)))
780 (and x
781 (begin
782 (u8vector-set! v i x)
783 (loop (- i 1)))))
784 v))))))))
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)
793 (define ERR_MASK 2)
794 (define RX_MASK 4)
795 (define CLOCK_MASK 8)
796 (define TX_MASK 128)
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)))
807 '
808 (if (and debug? trace?)
809 (pp (list '(rs232-read-line) '-> x)))
810 (if (eof-object? x)
811 (begin
812 (set! no-response-count (+ no-response-count 1))
813 (if (> no-response-count 100)
814 (begin
815 (pp 'base-station-not-responding)
816 (set! no-response-count 50))))
817 (begin
818 (if (and debug? trace?)
819 (begin
820 (display "<- ")
821 (display x)
822 (display "\n")))
823 (if (>= no-response-count 50)
824 (pp 'base-station-now-ok))
825 (set! no-response-count 0)))
826 x))
828 (define (rs232-send-no-check str trace?)
829 '
830 (if (and debug? trace?)
831 (pp (list 'rs232-send-no-check str)))
832 (display str rs232)
833 (display "\r" rs232)
834 (force-output rs232)
835 (if (and debug? trace?)
836 (begin
837 (display "-> ")
838 (display str)
839 (display "\n"))))
841 (define (rs232-send-no-cr-no-check str trace?)
842 '
843 (if (and debug? trace?)
844 (pp (list 'rs232-send-no-cr-no-check str)))
845 (display str rs232)
846 (force-output rs232)
847 (if (and debug? trace?)
848 (begin
849 (display "-> ")
850 (display str)
851 (display "\n"))))
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))
857 (begin
858 (display "<- ")
859 (display echo)
860 (display "\n")))
861 (and (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?)))
867 (and (string? echo)
868 (string=? echo str))))
870 (define (serve connection)
871 (thread-start!
872 (make-thread
873 (lambda ()
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)))
883 (mutex-lock! mutex)
884 (let ((client (vector-ref clients id)))
885 (if client
886 (begin
887 (mutex-unlock! mutex)
888 (display
889 (list "============================================= connection to robot " id " from " hostname " **REFUSED**\n"))
890 (if log-file
891 (begin
892 (display
893 (list "============================================= connection to robot " id " from " hostname " **REFUSED**\n")
894 log-file)
895 (force-output log-file)))
896 (close-port connection))
897 (begin
898 (vector-set! clients id connection)
899 (mutex-unlock! mutex)
900 (display
901 (list "============================================= connection to robot " id " from " hostname "\n"))
902 (if log-file
903 (begin
904 (display
905 (list "============================================= connection to robot " id " from " hostname "\n")
906 log-file)
907 (force-output log-file)))
908 (send '(ok) connection)
909 (process-client-commands connection id)
910 (mutex-lock! mutex)
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
917 (lambda (exc)
918 #f)
919 (lambda ()
920 (let loop ()
921 (let ((cmd (read connection)))
922 (if (vector? cmd)
923 (begin
924 (send (vector id cmd) multiplexer)
925 (loop))))))))
927 ;------------------------------------------------------------------------------