Infanoid | Keepon | ClayBot
日本語 | English
PC から Movado を動かす

ホスト PC から Movado を介してモータを動かすには,PC のシリアル通信ポートから Movado へ指令パケットを送信(また必要に応じて Movado から返信パケットを受信)することが必要です.それを実現するには,たとえば以下にあげる2つの方法が考えられます.

ここでは,これら2つの方法について解説します.エンジニア(手に機械油がついている方など)には前者を,アーティスト(手に絵の具がついている方など)には後者をおすすめします.

C によるプログラミング
【準備編:シリアル通信ポートの操作】

まず,シリアル通信ポート(/dev/ttyUSB0 や COM3 など)の制御やデータ送受信を行なうための基本インタフェースとして,以下の 5 つの関数を準備します.

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_init は,パス名 dev によって参照されるシリアル通信ポートを開き,Movado の通信プロトコルに合わせて初期設定を行ないます.com_quit はシリアル通信ポートを閉じます.com_send は n バイトのデータを送信し,com_send_byte は 1 バイトのデータを送信します.com_rec_byte は 1 バイトのデータを受信します.受信に成功した場合は 1 を,失敗(500ms で時間切れ)の場合は 0 を返します.いずれの関数も,致命的なエラーの場合は異常終了します.

これら基本インタフェースの実装方法は,Linux,Mac,Windows など,使用するオペレーティングシステムによって異なります.以下に,Linux(Ubuntu 8.10, 9.04)および Mac OS X(Leopard)の場合,Windows XP(Win32 API)の場合に分けて,実装例を紹介します.

com.c : Linux および 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 : 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;
}

com_init に与えるデバイス名は,Linux の場合は,たとえば "/dev/ttyUSB0" あるいは "/dev/ttyS0",Mac の場合は,たとえば "/dev/tty.KeySerial1",そして Windows の場合は,たとえば "COM3" のように指定します.Windows では,コントロール パネル > システム > ハードウェア > デバイス マネージャ > ポートを参照して,適切なデバイス名を見つけてください.

【実装編:パケットの送受信】

Movado への指令(および Movado からの返信)は 6 バイトのパケットの形をとります.その先頭バイトの MSB を 1 とし,後続するバイトの MSB を 0 とすることで,パケットのフレーム同期を保証しています.この部分を実装すると,つぎにあげる seq.c のようになります.

seq.c : Linux・Mac・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++;
    }
}

Movado のパケットは,アドレス(addr)・モータ番号(n)・命令(inst)・4 バイト引数(quad)からなります.アドレスは 5 ビット,モータ番号は 2 ビット,命令は 3 ビット,4 バイト引数は 32 ビットの整数データです.これら整数データとの変換部分を実装すると,つぎにあげる pac.c のようになります.なお,パケットの詳細構造については「制御ソフト Movado」のページを参照してください.

pac.c : Linux・Mac・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);
}

pac_send によって送信した指令パケットのうち,inst が 5(getacc)・6(getvel)・7(getpos)のいずれかであるか,あるいは inst が 4(sub)かつ quad の MSB が 1 のもの(get_upper, get_lower, get_word など)については,Movado から返信パケットがホスト PC に返されますので,pac_rec によって受信する必要があります.それ以外の場合は,返信パケットが返されません.pac_do は,返信パケットの有無を考慮したパケット処理を実装したものです.

【確認編:PC から Movado を動かす】

上述の com.c, seq.c, pac.c(とくに関数 com_init, com_quit, pac_do)をとおして,ホスト PC から Movado を自由に操作することができます.そこで,これら3つのファイルを連結し,Linux・Mac・Windows で共通して使えるようにしたプログラム c-movado.c を用意しました.その利用方法は,下にあげたテストプログラム c-movado-test.c を参考にしてください.

c-movado-test.c : Linux・Mac・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;
}

以下は,Linux および Mac でのコンパイル・実行例です.quad は 32 ビット固定小数点形式(整数部 16 ビット・小数部 16 ビット)で表現されています.この例では,addr=1, n=0 のモータについて,位置 64.0 [rad] への回転を指令し,2 秒後に現在位置を問い合わせたところ,63.948 [rad] の返答がありました.つづいて位置 0.0 [rad] への回転を指令し,2 秒後に現在位置を問い合わせたところ,0.037 [rad] の返答がありました.

コンパイルおよび実行の様子
unix> cc c-movado-test.c -o c-movado-test
unix> c-movado-test
getpos: 003ff2e2
getpos: 0000096c
unix>

Windows 上では,Microsoft Visual C++ 2008 Express Edition 上で Win32 コンソールアプリーケションとしてビルドし,同様の動作を確認しました.この開発環境は,Microsoft から無償で提供されています.

【応用編:各種指令の実装】

たとえば move 指令は,目標位置(単位は rad)を 32 ビット固定小数点データ(整数部 16 ビット・小数部 16 ビット)で与えます.しかしホスト PC 内部では,ふつう浮動小数点形式(たとえば double)で位置・速度などのデータを扱うことが多いので,このままでは不便です.そこで,double 形式で位置・速度などを指定できる機能を考えます.(ここでは単位を rad としています.)

c-movado-move.c : Linux・Mac・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);
}

また,Movado からの返信についても,たとえば getpos 指令によって得られる現在位置は 32 ビット固定小数点形式(整数部 16 ビット・小数部 16 ビット)によって表現されています.これについても,つぎのような関数を用意して,ホスト PC で処理しやすい double 形式で取得できると便利です.

c-movado-getpos.c : Linux・Mac・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.c をより使いやすいインタフェースにしたものが,つぎにあげる c-movado-link.c です.インタフェース関数の一覧もあげておきます.これが CareBots プロジェクトにおける公式の C-Movado インタフェースです.

C-Movado インタフェース関数一覧
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);
Max あるいは Pd によるプログラミング

Max はデータフロー型のプログラミングによるマルチメディア制作環境です.処理単位となる「オブジェクト」を相互にリンクすることで,全体的なデータ処理の流れを「パッチャー」とよばれる図式として視覚的・対話的にプログラミングしていきます.Max は Cycling'74 から Max/MSP/Jitter というパッケージとして販売されています.現在のところ Mac OS X と Windows XP/Vista に対応しています.また,Max の基本機能の多くは,同じ開発者によるオープンソースのソフトウェア Pd (Pure Data) でも利用することができます.Pd は Mac・Windows だけでなく Linux にも対応しています.

【Max によるプログラミング】

max-movado は,Movado への指令パケットの送信(および Movado からの返信パケットの受信)を行なうための Max プログラムです.つぎにあげる Max パッチャー max-movado.maxpat をダウンロードし,Max から参照パスの通ったフォルダに格納することで,max-movado を利用することができます.

Max 上で新しいパッチャーを作成し,そこに新規オブジェクトをつくり,その名前を max-movado としてください.これで max-movado オブジェクトが生成されます.参照パスが正しく設定されていれば,max-movado オブジェクトの上側にはインレット(入力端子),下側にはアウトレット(出力端子)が見えるはずです.

図:max-movado オブジェクト

上の例では max-movado に引数 a を与えてありますが,これは max-movado が使用するシリアルポートの識別子です.まず引数なしで max-movado を生成すると,Max Window にシリアルポートのデバイス名と識別子がリストされます.このリストから使用するシリアルポートを見つけ,その識別子を max-movado の引数として加えてください.

この max-movado オブジェクトに,指令パケットに相当するメッセージを送り込むことで,Max から Movado を制御することができます.たとえば,アドレス(addr)が 1,モータ番号(n)が 0 のモータを,位置 100.0 [rad] まで台形速度制御によって回転させることを考えます.まず,(1 0 move 100.) というメッセージを用意し,そのアウトレットから max-movado のインレットへのリンクを張ります.パッチャーの編集モードを終了し,このメッセージをクリックすると,その内容が max-movado のインレットに送り込まれます.その結果,目的とするモータは位置 100.0 [rad] まで回転します.

図:メッセージを max-movado へ接続

同じように,いくつかのメッセージを用意し,それを max-movado へ送信することで,モータをさまざまな位置まで回転させることができます.つぎの例では,モータを位置 100.0 [rad],0.0 [rad],−100.0 [rad] に動かします.

図:3つのメッセージを max-movado へ接続

max-movado のアウトレットからは,Movado からの返信パケットがメッセージとして出力されます.メッセージが出力されるのは,get 系の指令と一部の副指令の場合で,その他の場合は何も出力されません.これを確かめるために,新しく空のメッセージを生成し,その右インレット(代入用)に max-movado のアウトレットからのリンクを接続してみます.

図:max-movado から返信メッセージを受け取る

move 指令によってモータが動きますが,max-movado からは何も出力されません.一方,getpos 指令を送信すると,max-movado から現在位置を格納した返信メッセージが出力されます.上の例では,(1 0 move 100.) の後で (1 0 getpos) を max-movado に送り込んだところ,(1 0 getpos 100.003555) という返信メッセージが得られています.

このように max-movado とメッセージをやりとりすることで,Max から Movado を自由に操ることができます.とにかく max-movado の使い方を身体で覚えたい方は,つぎにあげる max-movado-help.maxpat を参照してください.

図:Max/MSP による max-movado-help パッチャー

max-movado のメッセージについて詳しく知りたい方は,まず下表にあげるメッセージ構造(シンタックス)のリストをご覧ください.それぞれの意味(セマンティクス)については「制御ソフト Movado」を参照してください.

さらに max-movado の内部処理を概観したい方は,下に max-movado パッチャーを図示しましたので,参考にしてください.左上端にインレット,左下端にアウトレット,赤色の部分が Movado に送信される 6 バイトのパケット,青色の部分が Movado から受信された 6 バイトのパケットです.黄色の serial というオブジェクトはシリアル通信ポートに相当するオブジェクト(Max 標準)です.

図:Max/MSP による max-movado パッチャー
【Pd によるプログラミング】

pd-movado は,Movado への指令パケットの送信(および Movado からの返信パケットの受信)を行なうための Pd プログラムで,機能拡張版の Pd-extended での利用を前提としています.つぎにあげる Pd パッチ pd-movado.pd をダウンロードし,Pd から参照パスの通ったフォルダに格納することで,pd-movado を利用することができます.

pd-movado の使い方(入出力メッセージの形式と意味)は max-movado と全く同じです.前出の「Max によるプログラミング」を参照してください.とにかく pd-movado の使い方を身体で覚えたい方は,つぎにあげる pd-movado-help.pd を参照してください.(英字でなく数字でシリアル通信ポートを指定することに注意してください.)

図:Pd による pd-movado-help パッチャー

pd-movado の内部処理を概観したい方は,下に pd-movado パッチャーを図示しましたので,参考にしてください.左上端にインレット,左下端にアウトレット,中央付近の comport というオブジェクトがシリアル通信ポートに相当するオブジェクト(Pd-extended 標準)です.

図:Pd による pd-movado パッチャー