Gambit Scheme FFI Notes

This is yet another post for personal reference, I've been playing with Gambit Scheme on the BeagleBoard, following is a collection of snippets that wraps OpenCV and POSIX Serial API using Gambit Scheme's FFI.

(c-declare "#include \"cv.h\"")
(c-declare "#include \"highgui.h\"")

(define CV_WINDOW_AUTOSIZE 1)

(define capture-from-cam (c-lambda (int) (pointer "CvCapture") 
                                   "cvCaptureFromCAM"))
(define named-window (c-lambda (char-string int) int "cvNamedWindow"))
(define destroy-window (c-lambda (char-string) void "cvDestroyWindow"))
(define query-frame (c-lambda ((pointer "CvCapture")) (pointer "IplImage") 
                              "cvQueryFrame"))
(define show-image (c-lambda (char-string (pointer "IplImage")) void 
                             "cvShowImage"))
(define release-capture (c-lambda 
                         ((pointer "CvCapture")) void 
                         "CvCapture* c = ___arg1; cvReleaseCapture(&c);"))
(define wait-key (c-lambda (int) int "cvWaitKey"))

Calling native functions is straight forward,

(define (display-webcam)
  (let ((camera (capture-from-cam 0))
        (window (named-window "cam" CV_WINDOW_AUTOSIZE)))
    (let loop ((n 1))
      (if (not (= n 113))
          (begin
            (show-image "cam" (query-frame camera))
            (loop (wait-key 1)))))
    (destroy-window "cam")))

(display-webcam)

/opt/local/bin/gambit-gsc -cc-options "-I/opt/local/include/opencv/" \
       -ld-options "-L/opt/local/lib -lopencv_core -lopencv_highgui" \
       -exe -o opencv opencv.scm

Returning scheme objects from a c-lambda is a bit more cumbersome, following will convert an IplImage to jpeg and return a list of bytes. (Just a reminder, cvEncodeImage is an internal call don't rely on it.)

(define ipl-jpeg
  (c-lambda ((pointer "IplImage")) scheme-object
#<<c-lambda-end
int jpeg_params[] = { CV_IMWRITE_JPEG_QUALITY, 80, 0 };
// buf is now a single-row matrix of type CV_8UC1 that contains the encoded image
CvMat* buf = cvEncodeImage(".jpeg", ___arg1, jpeg_params);

___SCMOBJ list,item,tmp;
list = ___NUL;

int col = 0;

for(col = buf->cols - 1; col >= 0; col-- ) {
        uchar* ptr = (uchar*)(buf->data.ptr + col);
        ___EXT(___U8_to_SCMOBJ) (ptr[0], &item, ___STILL);
        tmp  = ___EXT(___make_pair) (item,list, ___STILL);
        ___EXT(___release_scmobj) (list);
        list = tmp;
        }

___EXT(___release_scmobj) (item);
___EXT(___release_scmobj) (list);
cvReleaseMat(&buf);
___result = list;
c-lambda-end
))

And finally a servo only Firmata implementation,

(c-declare "#include \"termios.h\"")
(c-declare "#include \"fcntl.h\"")

(define serial-init 
  (c-lambda 
   (char-string) int 
#<<c-lambda-end
  struct termios toptions;
  int fd;

  fd = open(___arg1, O_RDWR | O_NOCTTY | O_NDELAY);

  if (fd == -1){
    perror("init_serialport: Unable to open port");
    ___result = -1;
   }else{

    if (tcgetattr(fd, &toptions) < 0) {
      perror("init_serialport: Couldn't get term attributes");
      ___result = -1;
    }else{

      cfsetispeed(&toptions, B57600);
      cfsetospeed(&toptions, B57600);

      // 8N1
      toptions.c_cflag &= ~PARENB;
      toptions.c_cflag &= ~CSTOPB;
      toptions.c_cflag &= ~CSIZE;
      toptions.c_cflag |= CS8;
      // no flow control
      toptions.c_cflag &= ~CRTSCTS;

      toptions.c_cflag |= CREAD | CLOCAL;  // turn on READ & ignore ctrl lines
      toptions.c_iflag &= ~(IXON | IXOFF | IXANY); // turn off s/w flow ctrl

      toptions.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG); // make raw
      toptions.c_oflag &= ~OPOST; // make raw

      toptions.c_cc[VMIN]  = 0;
      toptions.c_cc[VTIME] = 20;

      if( tcsetattr(fd, TCSANOW, &toptions) < 0) {
        perror("init_serialport: Couldn't set term attributes");
        ___result = -1;
      }else
        ___result = fd;
    }
   }
c-lambda-end
))

(define serial-write 
  (c-lambda (int int) void 
#<<c-lambda-end

char b[1]; 
b[0] = (char)___arg2; 
write(___arg1, b, 1);

c-lambda-end
))

(define SET-PIN-MODE #xF4) ;;set a pin to INPUT/OUTPUT/PWM/etc
(define ANALOG-MESSAGE #xE0) ;;send data for an analog pin (or PWM)

(define SERVO 4) ;; attach servo to pin

(define (arduino port)
  (let ((conn (serial-init port)))
    (thread-sleep! 5)
    conn))

(define (set-pin-mode conn pin mode)
  (serial-write conn SET-PIN-MODE)
  (serial-write conn pin)
  (serial-write conn mode))

(define (analog-write conn pin val)
  (serial-write conn (bitwise-ior ANALOG-MESSAGE (bitwise-and pin #x0F)))
  (serial-write conn (bitwise-and val #x7F))
  (serial-write conn (arithmetic-shift val -7)))

(define *servo-pin* 11)

(define *arduino* (arduino "/dev/tty.usbserial-A600aeCj"))
(set-pin-mode *arduino* *servo-pin* SERVO)
(analog-write *arduino* *servo-pin* 90)

(display "Arduino ready..")(newline)

(let loop () 
  (let ((line (read-line))) 
    (analog-write *arduino* *servo-pin* (string->number line))
    (display (string-append "Angle: " line))(newline)
    (loop)))

/opt/local/bin/gambit-gsc -exe -o servo servo.scm && ./servo