;; G26 communication protocol
(defpackage :g26
(:use :common-lisp))
(in-package :g26)
(defclass g26-message ()
((logical-channel :initarg :logical-channel)
(command-code :initarg :command-code)
(data-length :initarg :data-length)
(data :initarg :data)
(crc-code :initarg :crc-code)))
(defparameter *crc-table*
(make-array 256
:element-type 'unsigned-word
:initial-contents
'(#x0000 #x1021 #x2042 #x3063 #x4084 #x50A5 #x60C6 #x70E7
#x8108 #x9129 #xA14A #xB16B #xC18C #xD1AD #xE1CE #xF1EF
#x1231 #x0210 #x3273 #x2252 #x52B5 #x4294 #x72F7 #x62D6
#x9339 #x8318 #xB37B #xA35A #xD3BD #xC39C #xF3FF #xE3DE
#x2462 #x3443 #x0420 #x1401 #x64E6 #x74C7 #x44A4 #x5485
#xA56A #xB54B #x8528 #x9509 #xE5EE #xF5CF #xC5AC #xD58D
#x3653 #x2672 #x1611 #x0630 #x76D7 #x66F6 #x5695 #x46B4
#xB75B #xA77A #x9719 #x8738 #xF7DF #xE7FE #xD79D #xC7BC
#x48C4 #x58E5 #x6886 #x78A7 #x0840 #x1861 #x2802 #x3823
#xC9CC #xD9ED #xE98E #xF9AF #x8948 #x9969 #xA90A #xB92B
#x5AF5 #x4AD4 #x7AB7 #x6A96 #x1A71 #x0A50 #x3A33 #x2A12
#xDBFD #xCBDC #xFBBF #xEB9E #x9B79 #x8B58 #xBB3B #xAB1A
#x6CA6 #x7C87 #x4CE4 #x5CC5 #x2C22 #x3C03 #x0C60 #x1C41
#xEDAE #xFD8F #xCDEC #xDDCD #xAD2A #xBD0B #x8D68 #x9D49
#x7E97 #x6EB6 #x5ED5 #x4EF4 #x3E13 #x2E32 #x1E51 #x0E70
#xFF9F #xEFBE #xDFDD #xCFFC #xBF1B #xAF3A #x9F59 #x8F78
#x9188 #x81A9 #xB1CA #xA1EB #xD10C #xC12D #xF14E #xE16F
#x1080 #x00A1 #x30C2 #x20E3 #x5004 #x4025 #x7046 #x6067
#x83B9 #x9398 #xA3FB #xB3DA #xC33D #xD31C #xE37F #xF35E
#x02B1 #x1290 #x22F3 #x32D2 #x4235 #x5214 #x6277 #x7256
#xB5EA #xA5CB #x95A8 #x8589 #xF56E #xE54F #xD52C #xC50D
#x34E2 #x24C3 #x14A0 #x0481 #x7466 #x6447 #x5424 #x4405
#xA7DB #xB7FA #x8799 #x97B8 #xE75F #xF77E #xC71D #xD73C
#x26D3 #x36F2 #x0691 #x16B0 #x6657 #x7676 #x4615 #x5634
#xD94C #xC96D #xF90E #xE92F #x99C8 #x89E9 #xB98A #xA9AB
#x5844 #x4865 #x7806 #x6827 #x18C0 #x08E1 #x3882 #x28A3
#xCB7D #xDB5C #xEB3F #xFB1E #x8BF9 #x9BD8 #xABBB #xBB9A
#x4A75 #x5A54 #x6A37 #x7A16 #x0AF1 #x1AD0 #x2AB3 #x3A92
#xFD2E #xED0F #xDD6C #xCD4D #xBDAA #xAD8B #x9DE8 #x8DC9
#x7C26 #x6C07 #x5C64 #x4C45 #x3CA2 #x2C83 #x1CE0 #x0CC1
#xEF1F #xFF3E #xCF5D #xDF7C #xAF9B #xBFBA #x8FD9 #x9FF8
#x6E17 #x7E36 #x4E55 #x5E74 #x2E93 #x3EB2 #x0ED1 #x1EF0)))
(defparameter *qload*
(make-instance 'g26-message
:logical-channel #x00
:command-code #x01
:data-length #x0b
:data (make-array 11 :element-type 'unsigned-byte :initial-element 0)
:crc-code #x0000))
(defparameter *status-general*
(make-instance 'g26-message
:logical-channel #x04
:command-code #x10
:data-length #x00
:data nil
:crc-code #x0000))
(defparameter *status-motors*
(make-instance 'g26-message
:logical-channel #x04
:command-code #x11
:data-length #x00
:data nil
:crc-code #x0000))
(defparameter *status-tray1*
(make-instance 'g26-message
:logical-channel #x04
:command-code #x12
:data-length #x00
:data nil
:crc-code #x0000))
(defparameter *status-tray2*
(make-instance 'g26-message
:logical-channel #x04
:command-code #x13
:data-length #x00
:data nil
:crc-code #x0000))
(defparameter *status-tray3*
(make-instance 'g26-message
:logical-channel #x04
:command-code #x14
:data-length #x00
:data nil
:crc-code #x0000))
(defparameter *status-processes*
(make-instance 'g26-message
:logical-channel #x04
:command-code #x15
:data-length #x00
:data nil
:crc-code #x0000))
(defparameter *status-peltier*
(make-instance 'g26-message
:logical-channel #x04
:command-code #x16
:data-length #x00
:data nil
:crc-code #x0000))
(defparameter *status-photometer*
(make-instance 'g26-message
:logical-channel #x04
:command-code #x17
:data-length #x01
:data (make-array 1 :element-type 'unsigned-byte :initial-element 0)
:crc-code #x0000))
(defparameter *status-outputs*
(make-instance 'g26-message
:logical-channel #x04
:command-code #x18
:data-length #x00
:data nil
:crc-code #x0000))
(defparameter *status-tray1*
(make-instance 'g26-message
:logical-channel #x04
:command-code #x12
:data-length #x00
:data nil
:crc-code #x0000))
(defparameter *version-message*
(make-instance 'g26-message
:logical-channel #x80
:command-code #x19
:data-length #x00
:data nil
:crc-code #x0000))
(defparameter *operator-go*
(make-instance 'g26-message
:logical-channel #x10
:command-code #x20
:data-length #x00
:data nil
:crc-code #x0000))
(defparameter *operator-kill*
(make-instance 'g26-message
:logical-channel #x10
:command-code #x21
:data-length #x01
:data (make-array 1 :element-type 'unsigned-byte :initial-element 0)
:crc-code #xffff))
(defparameter *ack-message*
(make-instance 'g26-message
:logical-channel #x20
:command-code #x80
:data-length #x01
:data (make-array 1 :element-type 'unsigned-byte :initial-element 0)
:crc-code #xffff))
(defparameter *nak-message*
(make-instance 'g26-message
:logical-channel #x20
:command-code #x83
:data-length #x01
:data (make-array 1 :element-type 'unsigned-byte :initial-element 0)
:crc-code #xffff))
;; open a serial port-like device
(defun open-serial-port (tty-name)
(unless (stringp tty-name)
(error "open-serial-port: wrong device name."))
(sb-posix:open tty-name (logior sb-posix:O-RDWR sb-posix:O-NOCTTY sb-posix:O-NONBLOCK sb-posix:O-NDELAY)))
;; close a special file
(defun close-serial-port (fd)
(when (< fd 0)
(error "close-serial-port: wrong file descriptor value."))
(sb-posix:fcntl fd sb-posix:F-SETFL 0)
(sb-posix:close fd))
(defun get-serial-port-config (fd)
(when (< fd 0)
(error "get-serial-port-config: wrong file descriptor value."))
(sb-posix:tcgetattr fd))
(defun set-serial-port-config (fd config &key iflag oflag cflag lflag)
(when (< fd 0)
(error "set-serial-port-config: serial port not open."))
(cond
((not (eql iflag nil)) (setf (sb-posix:termios-iflag config) iflag))
((not (eql oflag nil)) (setf (sb-posix:termios-oflag config) oflag))
((not (eql cflag nil)) (setf (sb-posix:termios-cflag config) cflag))
((not (eql lflag nil)) (setf (sb-posix:termios-lflag config) lflag)))
(sb-posix:tcsetattr fd sb-posix:TCIFLUSH config)
config)
;
; g26 message to unsigned byte array
;
(defun message-to-array (message)
(unless (null message)
(let ((message-body (make-array (+ (slot-value message 'data-length) 5) :element-type 'unsigned-byte :initial-element 0)))
(setf (aref message-body 0) (slot-value message 'logical-channel))
(setf (aref message-body 1) (slot-value message 'command-code))
(setf (aref message-body 2) (slot-value message 'data-length))
(replace message-body (slot-value message 'data) :start1 3)
(setf (aref message-body (+ (slot-value message 'data-length) 3)) (logand (slot-value message 'crc-code) #xff))
(setf (aref message-body (+ (slot-value message 'data-length) 4)) (logand (ash (slot-value message 'crc-code) -8) #xff))
message-body)))
;
; unsigned byte array to g26 message
;
(defun array-to-message (message-body)
(unless (null message-body)
(when (arrayp message-body)
(when (> (length message-body) 4)
(let ((message (make-instance 'g26-message)))
(setf (slot-value message 'logical-channel) (aref message-body 0))
(setf (slot-value message 'command-code) (aref message-body 1))
(setf (slot-value message 'data-length) (aref message-body 2))
(setf (slot-value message 'data) (make-array (aref message-body 2) :element-type 'unsigned-byte))
(replace (slot-value message 'data) message-body :start2 3)
(setf (slot-value message 'crc-code) (logior (aref message-body (+ (aref message-body 2) 3)) (ash (aref message-body (+ (aref message-body 2) 4)) 8)))
message)))))
;
; receive g26 message
;
(defun get-message (fd)
(when (null fd)
(error "get-message: serial port not open."))
(let ((message-body (make-array 261 :element-type 'unsigned-byte :initial-element 0)))
(when (= (read-sequence message-body fd) 3)
(when (= (read-sequence message-body fd :start 3) (+ (aref message-body 2) 2))
(crc16 (subseq message-body 0 (- (length message-body) 2)))
(let ((message (array-to-message message-body)))
message)))))
;
; send a message to g26
;
(defun send-message (fd message)
(when (null fd)
(error "send-message: serial port not open."))
(when (null message)
(error "send-message: wrong message."))
(let ((message-body (message-to-array message)))
(loop for i from 0 below (length message-body) do
(write-byte (aref message-body i) fd))
(length message-body)))
;
; start it all
;
(defun start-communication (serial-port-name)
(let ((serial-port-descriptor (open-serial-port serial-port-name)))
(when (< serial-port-descriptor 0)
(error "start-communication: serial port not open."))
(let ((serial-port-config (get-serial-port-config serial-port-descriptor)))
(setf (sb-posix:termios-cflag serial-port-config) (logior sb-posix:b115200 sb-posix:PARENB sb-posix:PARODD))
(set-serial-port-config serial-port-descriptor serial-port-config)
;
; let's communicate with g26
;
(let ((serial-port-stream (sb-sys:make-fd-stream serial-port-descriptor :element-type 'unsigned-byte)))
(send-message serial-port-stream *status-general*)
(close serial-port-stream)
(close-serial-port serial-port-descriptor)))))
;
; crc16 calculation
;
(defun crc16 (buffer)
(unless (null buffer)
(when (arrayp buffer)
(let ((crc-value 0))
(loop for i from 0 below (length buffer) do
(setf crc-value (logand (logxor (aref *crc-table* (logxor (ash crc-value -8) (aref buffer i))) (ash crc-value 8)) #xffff)))
crc-value))))
Thanks to smart people on #lisp: Xach, stassat, H4ns and the others :)
G26 it's a smart robot that performs biological/chemical manipulations on human fluids.
No comments:
Post a Comment