CareBots Project (ClayBot's Specification)

Controlling Movado from PC

Programming in C (for engineers)．
Programming in Max or Pd (for artists)．

Programming in C

Accessing the serial port

void  com_init (char *dev);
void  com_quit ();
void  com_send (unsigned char *data, int n);
void  com_send_byte (unsigned char data);
int   com_rec_byte (unsigned char *data);

com.c (for Linux and Mac)

#include <stdlib.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <sys/uio.h>
#include <unistd.h>
#include <termios.h>
#include <stdio.h>

static int  ComFD = -1;                 //  file descriptor

void  com_error (char *func, char *mess)
{
    fprintf(stderr, "*** com_%s: %s\n", func, mess);
    fprintf(stderr, "Press ENTER to exit: "); fflush(stderr);
    getchar();
    exit(1);
}

void  com_init (char *dev)
{
    static struct termios  termios_data;
    int  ret;

    //  open serial port
    ComFD = open(dev, O_RDWR | O_NOCTTY | O_NONBLOCK);
    if (ComFD < 0)
        com_error("init", "can't open the port");

    //  set attribute (settings and baud)
    ret = tcgetattr(ComFD, &termios_data);
    termios_data.c_iflag  = 0;
    termios_data.c_oflag  = 0;
    termios_data.c_lflag  = 0;
    termios_data.c_cflag  = CS8 | CREAD | CLOCAL;
    termios_data.c_cc[VMIN]  = 0;       //  "read" byte by byte
    termios_data.c_cc[VTIME] = 5;       //  timeout 500ms
    ret = cfsetspeed(&termios_data, B38400);
    if (ret < 0)
        com_error("init", "cannot set speed");
    ret = tcsetattr(ComFD, TCSANOW, &termios_data);
    if (ret < 0)
        com_error("init", "cannot set attribute");

    //  turn off "NONBOLCK"
    ret = fcntl(ComFD, F_SETFL, 0);
    if (ret < 0)
        com_error("init", "can't fcntl (to unset NONBLOCK)");
  
    //  flush input/output
    ret = tcflush(ComFD, TCIOFLUSH);
    if (ret < 0)
        com_error("init", "cannot flush");
}

void  com_quit ()
{
    int  ret;

    //  close the port
    ret = close(ComFD);
    if (ret < 0)
        com_error("quit", "cannot close the port");
}

void  com_send (unsigned char *data, int n)
{
    int  ret, ret_drain;

    //  write data to the port
    ret = write(ComFD, data, n);
    if (ret < 0)
        com_error("send", "cannot write data to the port");

    //  drain out the write-buffer
    ret_drain = tcdrain(ComFD);
    if (ret_drain < 0)
        com_error("send", "cannot drain the data");

    //  incomplete?
    if (ret < n) {
        //  still some data to send
        com_send(data + ret, n - ret);
    }
}

void  com_send_byte (unsigned char data)
{
    com_send(&data, 1);
}

int  com_rec_byte (unsigned char *data)
{
    int  ret;

    //  read data from the port
    ret = read(ComFD, data, 1);
    if (ret < 0)
        com_error("rec_byte", "cannot read byte from the port");

    //  return 1 on success
    //  return 0 on timeout
    return ret;
}

com.c (for Windows)

#include <windows.h>
#include <stdio.h>

HANDLE  ComFH;                          //  file handle

void  com_error (char *func, char *mess)
{
    fprintf(stderr, "*** com_%s: %s\n", func, mess);
    fprintf(stderr, "Press ENTER to exit: "); fflush(stderr);
    getchar();
    exit(1);
}

void  com_init (char *dev)
{
    DCB  dcb;                           //  device settings
    COMMTIMEOUTS  t_out;                //  timeout settings
    int  ret;

    //  open serial port
    ComFH = CreateFile(dev, GENERIC_READ | GENERIC_WRITE, 0, 0,
                       OPEN_EXISTING, FILE_ATTRIBUTE_NORMAL, 0);
    if (ComFH == INVALID_HANDLE_VALUE)
        com_error("init", "cannot CreateFile");

    //  set attribute (baud, etc.)
    FillMemory(&dcb, sizeof(dcb), 0);
    dcb.DCBlength = sizeof(dcb);
    ret = BuildCommDCB("38400,n,8,1", &dcb);
    if (ret == 0)
        com_error("init", "cannot BuildCommDCB");
    dcb.fRtsControl = RTS_CONTROL_DISABLE;
    ret = SetCommState(ComFH, &dcb);
    if (ret == 0)
        com_error("init", "cannot SetCommState");

    //  set timeout (500ms)
    t_out.ReadIntervalTimeout         = MAXDWORD;
    t_out.ReadTotalTimeoutMultiplier  = MAXDWORD;
    t_out.ReadTotalTimeoutConstant    = 500;
    t_out.WriteTotalTimeoutMultiplier = MAXDWORD;
    t_out.WriteTotalTimeoutConstant   = 500;
    ret = SetCommTimeouts(ComFH, &t_out);
    if (ret == 0)
        com_error("init", "cannot SetCommTimeouts");
}

void  com_quit ()
{
    int  ret;

    ret = CloseHandle(ComFH);
    if (ret == 0)
        com_error("quit", "cannot CloseHandle");
}

void  com_send (unsigned char *data, int n)
{
    int  ret;
    DWORD  n_done;

    ret = WriteFile(ComFH, data, n, &n_done, NULL);
    if (ret == 0)
        com_error("send", "cannot WriteFile");
    if ((int) n_done < n) {
        //  still some data to send
        Sleep(100);
        com_send(data + n_done, n - n_done);
    }
}

void  com_send_byte (unsigned char data)
{
    com_send(&data, 1);
}

int  com_rec_byte (unsigned char *data)
{
    int  ret;
    DWORD  n_done;

    //  read data from the port
    ret = ReadFile(ComFH, data, 1, &n_done, NULL);
    if (ret == 0)
        com_error("rec_byte", "cannot ReadFile");

    //  return 1 on success
    //  return 0 on timeout
    return n_done;
}

Sending and receiving packets

seq.c (for Linux, Mac, and Windows)

#include "com.c"

void  seq_send (unsigned char *packet)
{
    //  set marker for "beginning-of-packet"
    packet[0] |= 0x80;
    //  send a six-byte packet
    com_send(packet, 6);
}

void  seq_rec (unsigned char *packet)
{
    int  i;

    //  find "beginning-of-packet"
    do {
        com_rec_byte(&(packet[0]));
    } while (packet[0] < 0x80);

    //  read the packet body
    i = 1;
    while (i < 6) {
        com_rec_byte(&(packet[i]));
        if (packet[i] >= 0x80) {
            //  unexpected restart
            packet[0] = packet[i];
            i = 0;
        }
        i++;
    }
}

pac.c (for Linux, Mac, and Windows)

#include "seq.c"

void  pac_send (int addr, int n, int inst, int quad)
{
    unsigned char  data[6];
  
    data[0] = 0x80 | ((addr & 0x1f) << 2) | (n & 0x03);
    data[1] = ((inst & 0x07) << 4)
            | ((quad & 0x80000000)? 0x08: 0)
            | ((quad & 0x00800000)? 0x04: 0)
            | ((quad & 0x00008000)? 0x02: 0)
            | ((quad & 0x00000080)? 0x01: 0);
    data[2] = (quad >> 24) & 0x7f;
    data[3] = (quad >> 16) & 0x7f;
    data[4] = (quad >>  8) & 0x7f;
    data[5] = quad         & 0x7f;

    seq_send(data);
}

void  pac_rec (int *addr, int *n, int *inst, int *quad)
{
    unsigned char  data[6];
  
    seq_rec(data);

    *addr = (data[0] & 0x7c) >> 2;
    *n    = (data[0] & 0x03);
    *inst = (data[1] >> 4);
    *quad = ((data[2] | ((data[1] & 0x08)? 0x80: 0)) << 24)
          | ((data[3] | ((data[1] & 0x04)? 0x80: 0)) << 16)
          | ((data[4] | ((data[1] & 0x02)? 0x80: 0)) <<  8)
          | ((data[5] | ((data[1] & 0x01)? 0x80: 0)));
}

void  pac_do (int addr, int n, int inst, int *quad)
{
    //  send command
    pac_send(addr, n, inst, *quad);
    //  receive response (if needed)
    if (inst >= 5 || ((inst == 4) && ((*quad) & 0x80000000)))
        pac_rec(&addr, &n, &inst, quad);
}

Controlling Movado from PC

c-movado.c : C-Movado fundamental interface (for Linux, Mac, and Windows)

c-movado-test.c : Test/sample program (for Linux, Mac, and Windows)

c-movado-test.c (for Linux, Mac, and Windows)

#include "c-movado.c"

#if defined(linux)
//  Linux
#define  DEF_DEV  "/dev/ttyUSB0"                    //  example
//
#elif defined(__APPLE__) && defined(__MACH__)
//  Mac OS X
#define  DEF_DEV  "/dev/cu.I-O DATA USB-RSAQ5"      //  example
//
#elif defined(_WIN32)
//  Windows
#define  DEF_DEV  "COM3"                            //  example
#define  sleep(s)  Sleep((s)*1000)
//
#endif

int  main (int argc, char **argv)
{
    int  addr = 1;
    int  n    = 0;
    int  quad;

    //  specify your serial port
    com_init(DEV_DEV);

    //  move 0x01:0 to 64.0 rad
    quad = 0x00400000;
    pac_do(addr, n, 0, &quad);
    sleep(2);
    //  getpos 0x01:0
    pac_do(addr, n, 7, &quad);
    printf("getpos: %08x\n", quad);
    //  move 0x01:0 to 0.0 rad
    quad = 0x00000000;
    pac_do(addr, n, 0, &quad);
    sleep(2);
    //  getpos 0x01:0
    pac_do(addr, n, 7, &quad);
    printf("getpos: %08x\n", quad);

    //  close serial port
    com_quit();

    return 0;
}

Example: Compilation and execution

unix> cc c-movado-test.c -o c-movado-test
unix> c-movado-test
getpos: 003ff2e2
getpos: 0000096c
unix>

Implementing the "Commands"

c-movado-move.c (for Linux, Mac, and Windows)

#include "c-movado.c"

void  movado_move (int addr, int n, double pos)
{
    int  quad;

    quad = pos * 65536;
    pac_do(addr, n, 0, &quad);
}

c-movado-getpos.c (for Linux, Mac, and Windows)

#include "c-movado.c"

double  movado_getpos (int addr, int n)
{
    int  quad;
    double  pos;

    pac_do(addr, n, 7, &quad);
    pos = quad / 65536.0;

    return pos;
}

c-movado-link.c : C-Movado interface (for Linux, Mac, and Windows)
c-movado-link-test.c : Test/sample program (for Linux, Mac, and Windows)

C-Movado interface functions

void  movado_move (int addr, int n, double pos);
void  movado_movac (int addr, int n, double pos, double acc);
void  movado_movel (int addr, int n, double pos, double vel);
void  movado_setacc (int addr, int n, double acc);
void  movado_setvel (int addr, int n, double vel);
void  movado_setpos (int addr, int n, double pos);
double  movado_getacc (int addr, int n);
double  movado_getvel (int addr, int n);
double  movado_getpos (int addr, int n);
int  movado_sub (int addr, int n, int jnst, int index, int word);

Programing in Max or Pd

Programing in Max

Download : max-movado.maxpat (Max-Movado interface patcher)

figure: linking 3 messages to max-movado

figure: receiving a message from max-movado

Download : max-movado-help.maxpat (Test/sample patcher of Max-Movado)

(addr n move pos)
(addr n movac acc pos)
(addr n movel vel pos)
(addr n setacc acc)
(addr n setvel vel)
(addr n setpos pos)
(addr n sub jnst index word)
(addr n getacc)
(addr n getvel)
(addr n getpos)

Programming in Pd-extended

Download : pd-movado.pd (Pd-Movado interface patch)

Download : pd-movado-help.pd (Test/sample patch of Pd-Movado)