Lesson1 プチロボをC言語で動かす

１－１　Dev-Cppをセットアップする

ここでは、初心者のためにDev-CppのIDEを用いずにコマンドラインでコンパイル実行させます。従って、環境変数のPATHにC:\Dev-Cpp\binを登録してください。(推奨機の場合は入っているはず)

１－２　サーボモーターを動かしてみる

// servo.c
// プチロボットのサーボモーターのテストプログラム 
// g++ でコンパイルする

#include <stdio.h>
#include <ctype.h>
#include "comm.h" 
#define COMPORT "COM5"

int main()
{
    int key, end=1;
    int position=110;
    int i, save;

    printf("プチロボットテストプログラム\n+:10 -:-10 space:1 other:-1 q:終了\n");
    if(initServo(COMPORT)) return -1;
    while(end) {
        if(kbhit()) {
            key=readkey();
            save = position;
            switch(key&0x7f){
                case '+': 
                case ';': position+=10; break;
                case '-': position-=10; break;
                case 'q': end=0; break;
                case ' ': position++; break;
                default:  position--;
            }
            if((position <1) || (220<position)) position = save;
            else {
                printf("%d\n", position);
                fflush(stdout);
            }
        }
        for(i=0; i<9; i++)
            servo(i, position);
    }
    closeServo();
}

コンパイル方法

g++ servo.c servokey.o -o servo

これをコンパイルするには、関連するヘッダーファイルとオブジェクトファイルservokey.oが必要になります。ここをクリックしてダウンロードして下さい。これらはソースファイルと同一のディレクトリにある必要があります。
このservokey.oはC++で作られているのでg++というC++のコンパイラでコンパイルします。

このプログラムは+-キーなどを押すと9個のサーボモーターが一斉に同じように動くプログラムで、通信系、モーターのチェック用に使うこともできます。

１－３　プチロボの動きを作る

キャプチャーしながら頑張る。

１－４　msfファイルを読む

１－４－１　msfファイルについて

動きパターンの基本動作をプチロボのソフトで作り、保存します。例えば、まっすぐ歩くという動作の場合、右足を出して右足を後ろに蹴ると共に左足を前に出して・・という一連の動きを作ります。この過程をwalkという名前でキャプチャーし、ファイルに保存するとC:\Program Files\Puchi_Roboにwalk.msfというファイル名で保存されます。下の内容は単なるフォーマットの例なので歩きません。

;walk
0+22,1-11,2+9,3-46,4+40,5+17,6+0,7+22,8+14
0+35,1+9,2+29,3-56,4+30,5+7,6-16,7+22,8+14
0+25,1-1,2+19,3-66,4+20,5-3,6-26,7+22,8+14
0+15,1-11,2+9,3-76,4+10,5-13,6-26,7+12,8+4
0+15,1-11,2+9,3-76,4+10,5-13,6-26,7-8,8-16
0+15,1-11,2+9,3-76,4+10,5-13,6-26,7-28,8-36
0+15,1-11,2+9,3-86,4+0,5-23,6-26,7-28,8-36

;walkはコメントでファイル名と一致します。

次の行からが実際に動かすパターンで、一つ一つのサーボモーターに送るデータがカンマで区切られています。一番頭のデータは0+22ですが、データの頭の数字0がサーボモーターの番号です。その次の+22はホームポジションから見て相対的に+方向に22の位置に移動しろという命令です。後は同じようにして他のサーボモータの移動命令になります
　ここで問題になるのは、どこがホームポジションか分からないことです。一番初めにホームポジションに移動させなければ全てが始まりません。この情報は、先ほどのwalk.msfのあったフォルダと同じ所にあるPuchi_Robo_Config.iniにあります。このファイルの中は例えば以下のようになっています。

[Comm]
Port=1
[Direction]
M0=+
M1=+
M2=+
M3=+
M4=+
M5=+
M6=+
M7=+
M8=+
[HomePosition]
M0=110
M1=110
M2=110
M3=110
M4=110
M5=110
M6=110
M7=110
M8=110
[Function]
Button3=walk
[FunctionPath]
Button3=C:\Program Files\Puchi_Robo\walk.msf

[HomePosition]から下のM0からM8までの値が各モーターのホームポジションの値です。全部110になっていますが、実際には色々な値になるはずです。可動範囲は大体１～220なので、この場合は全モーターが中心からスタートを意味します。

[Comm]は通信を行うポート番号です。

[Direction]はホームポジションからどちら周りを+と考えるかという設定です。+にすると時計周りとなります。

[Function]の定義のところには上で示したwalk.msfがボタン3に登録されていることを意味しますが、Cでプログラミングする場合には関係ありません。

１－４－２　初期設定ファイルを読んでサーボモーターをホームポジションに移動させる

/*
    read.c
    設定ファイルPuchi_Robo_Config.iniを読み込み、COMポート番号、
    ホームポジジョン、回転方向を配列に入れる。
    サーボモーターをホームポジションに移動させる。
*/
#include <stdio.h>
#include "comm.h" 

#define MotorNum 9
#define TRUE  1
#define FALSE 0

#define DEBUG

void initHome();
void initialization();

int direction[MotorNum];        // 回転方向
int home[MotorNum];             // ホームポジション
char COMport[10];
int now[MotorNum];              // モーターの現在位置
int port;

main()
{
    int i;

    initialization();
    sprintf(COMport, "COM%d", port);
#ifdef DEBUG
    printf("ComPort:%s\n", COMport);
#endif
    if(initServo(COMport)) return -1;
    // ホームポジションに移動
    initHome();
    closeServo();
}

void initialization()
{
    FILE *fp;
    char config[] = "Puchi_Robo_Config.ini";
    char buffer[100], *p;
    int i, j, position;
  
    fp = fopen(config, "r");
    while(fscanf(fp,"%s",buffer) >0) {
        if(buffer[0] !='[') continue;
        p = strchr(buffer,']');
        *p = 0;
        if(!strcmp(&buffer[1], "Comm")) {
            fscanf(fp,"%s",buffer);
            p = strchr(buffer,'=');
            *p++ = 0;
            port = atoi(p);
#ifdef DEBUG
            printf("Comm %d\n", port);
#endif
        }
        else if(!strcmp(&buffer[1], "HomePosition")) {
            for(i=0; i<MotorNum; i++) {
                fscanf(fp,"%s",buffer);
                if(buffer[0]=='M') {
                    p = strchr(buffer,'=');
                    *p++ = 0;
                    j = atoi(&buffer[1]);
                    if(j<MotorNum) 
                        home[j] = atoi(p);
                }
#ifdef DEBUG
                printf("HomePosition %s %d %d\n", buffer, j, home[j]);
#endif
            }
        }
        else if(!strcmp(&buffer[1], "Direction")) {
            for(i=0; i<MotorNum; i++) {
                fscanf(fp,"%s",buffer);
                if(buffer[0]=='M') {
                    p = strchr(buffer,'=');
                    *p++ = 0;
                    j = atoi(&buffer[1]);
                    if(j<MotorNum) 
                        direction[j] = (*p == '+') ? 1: -1;
                }
#ifdef DEBUG
                printf("Direction %s %d %d\n", buffer, j, direction[j]);
#endif
            }
        }
    }
    fclose(fp);
}

void initHome()
{
    int i, position;

    // ホームポジションに設定する
    for(i=0; i<MotorNum; i++) {
        position = (direction[i]==1) ? home[i] : 110 - (home[i] - 110);
        servo(i, position);
        now[i] = position;      // 現在位置
    }
}

課題

例えば下記のようなforward.msfを読み込んで、これを解析し実行する関数translationを作り、mainからこれを呼びロボットを動かしなさい。このファイル名はtrans.cとします。

;forward
S30
5+8,2+8
W300
1+10,0-25,4-10,3+15,8+40,7-40
W300
2-8,5-8
W300
1-10,0+25,4+10,3-15,8-40,7+73
W300
END

１－４－３　msfファイルを読んでみる

以下のソースをtrans1.cというファイル名で作成します。

msfファイルを読んでみる
#include <stdio.h> #include <string.h> #define MotorNum 9 void translation(char* filename); main() { translation("forward.msf"); } // ファイルを読み込んで解釈実行 void translation(char* filename) { FILE *fp; char buffer[100]; fp = fopen(filename, "r"); while(fscanf(fp,"%s",buffer) >0) { if(!strcmp(buffer,"END")) break; // 終了 // if(buffer[0]==';') continue; // コメント処理 // printf("%s\n", buffer); } fclose(fp); }

msfファイルを読んでみる

#include <stdio.h>
#include <string.h>

#define MotorNum 9

void translation(char* filename);

main()
{
    translation("forward.msf");
}

// ファイルを読み込んで解釈実行
void translation(char* filename)
{
    FILE *fp;
    char buffer[100];
   
    fp = fopen(filename, "r");
    while(fscanf(fp,"%s",buffer) >0) {
        if(!strcmp(buffer,"END")) break;    // 終了 //
        if(buffer[0]==';') continue;        // コメント処理 //
        printf("%s\n", buffer);
    }
    fclose(fp); 
}

１－４－４　スピードコマンドを解析する

以下のソースをtrans2.cというファイル名で作成します。

スピードコマンドを解析する
#include <stdio.h> #include <stdlib.h> #include <string.h> #define MotorNum 9 void translation(char* filename); main() { translation("forward.msf"); } // ファイルを読み込んで解釈実行 void translation(char* filename) { FILE *fp; char buffer[100]; int speed; fp = fopen(filename, "r"); while(fscanf(fp,"%s",buffer) >0) { if(!strcmp(buffer,"END")) break; // 終了 // if(buffer[0]==';') continue; // コメント処理 // if(buffer[0]=='S') { speed = atoi(&buffer[1]); printf("speed = %d\n", speed); } else printf("%s\n", buffer); } fclose(fp); }

スピードコマンドを解析する

#include <stdio.h>
#include <stdlib.h>
#include <string.h>

#define MotorNum 9

void translation(char* filename);

main()
{
    translation("forward.msf");
}

// ファイルを読み込んで解釈実行
void translation(char* filename)
{
    FILE *fp;
    char buffer[100];
    int speed;

    fp = fopen(filename, "r");
    while(fscanf(fp,"%s",buffer) >0) {
        if(!strcmp(buffer,"END")) break;    // 終了 //
        if(buffer[0]==';') continue;        // コメント処理 //

        if(buffer[0]=='S') {
            speed = atoi(&buffer[1]);
            printf("speed = %d\n", speed);
        }

        else printf("%s\n", buffer);
    }
    fclose(fp); 
}

１－４－５　waitコマンドを解析する

以下のソースをtrans3.cというファイル名で作成します。

waitコマンドを解析する
#include <stdio.h> #include <stdlib.h> #include <string.h> #define MotorNum 9 void translation(char* filename); main() { translation("forward.msf"); } // ファイルを読み込んで解釈実行 void translation(char* filename) { FILE *fp; char buffer[100]; int speed; fp = fopen(filename, "r"); while(fscanf(fp,"%s",buffer) >0) { if(!strcmp(buffer,"END")) break; // 終了 // if(buffer[0]==';') continue; // コメント処理 // if(buffer[0]=='S') { speed = atoi(&buffer[1]); printf("speed = %d\n", speed); } else if(buffer[0]=='W') { int wait; wait = atoi(&buffer[1]); printf("wait = %d\n", wait); // Sleep(wait); } else printf("%s\n", buffer); } fclose(fp); }

waitコマンドを解析する

#include <stdio.h>
#include <stdlib.h>
#include <string.h>

#define MotorNum 9

void translation(char* filename);

main()
{
    translation("forward.msf");
}

// ファイルを読み込んで解釈実行
void translation(char* filename)
{
    FILE *fp;
    char buffer[100];
    int speed;

    fp = fopen(filename, "r");
    while(fscanf(fp,"%s",buffer) >0) {
        if(!strcmp(buffer,"END")) break;    // 終了 //
        if(buffer[0]==';') continue;        // コメント処理 //
        if(buffer[0]=='S') {
            speed = atoi(&buffer[1]);
            printf("speed = %d\n", speed);
        }

        else if(buffer[0]=='W') {
            int wait;
            wait = atoi(&buffer[1]);
            printf("wait = %d\n", wait);
          //  Sleep(wait);
        }

        else printf("%s\n", buffer);
    }
    fclose(fp); 
}

Sleepを有効にするには#include <windows.h>を付け加える必要がありますが、これを含んでいるcomm.hを後でインクルードするので今のところここはコメントにしておきます。同様にstdlib.h、string.hも必要がなくなります。

１－４－６　スイッチ文に書き換える

以下のソースをtrans4.cというファイル名で作成します。

スイッチ文に書き換える
#include <stdio.h> #include <stdlib.h> #include <string.h> #define MotorNum 9 void translation(char* filename); main() { translation("forward.msf"); } // ファイルを読み込んで解釈実行 void translation(char* filename) { FILE *fp; char buffer[100]; int speed, wait; fp = fopen(filename, "r"); while(fscanf(fp,"%s",buffer) >0) { if(!strcmp(buffer,"END")) break; // 終了 // switch(buffer[0]) { case ';': continue; // コメント処理 // case 'S': speed = atoi(&buffer[1]); printf("speed = %d\n", speed); break; case 'W': wait = atoi(&buffer[1]); printf("wait = %d\n", wait); // Sleep(wait); break; default: printf("%s\n", buffer); } } fclose(fp); }

スイッチ文に書き換える

#include <stdio.h>
#include <stdlib.h>
#include <string.h>

#define MotorNum 9

void translation(char* filename);

main()
{
    translation("forward.msf");
}

// ファイルを読み込んで解釈実行
void translation(char* filename)
{
    FILE *fp;
    char buffer[100];
    int speed, wait;

    fp = fopen(filename, "r");
    while(fscanf(fp,"%s",buffer) >0) {
        if(!strcmp(buffer,"END")) break;    // 終了 //

        switch(buffer[0]) {
            case ';':   continue;        // コメント処理 //
            case 'S':   speed = atoi(&buffer[1]);
                        printf("speed = %d\n", speed);
                        break;
            case 'W':   wait = atoi(&buffer[1]);
                        printf("wait = %d\n", wait);
                        //  Sleep(wait);
                        break;
            default:    printf("%s\n", buffer);
        }

    }
    fclose(fp); 
}

１－４－７　サーボモーターの移動量コマンドを分離する

以下のソースをtrans5.cというファイル名で作成します。

サーボモーターの移動量コマンドを分離する
#include <stdio.h> #include <stdlib.h> #include <string.h> #define MotorNum 9 void translation(char* filename); main() { translation("forward.msf"); } int split(char buffer[],char p[]){ int n, m, j; char q; p[0] = buffer; m=j=1; do { q = strchr(&buffer[m],','); if(!q) break; n = q - buffer; buffer[n]=0; p[j++] = &buffer[m=++n]; } while(1); return j; } // ファイルを読み込んで解釈実行 void translation(char* filename) { FILE fp; char buffer[100]; char p[MotorNum]; int i, n, speed, wait; fp = fopen(filename, "r"); while(fscanf(fp,"%s",buffer) >0) { if(!strcmp(buffer,"END")) break; // 終了 // switch(buffer[0]) { case ';': continue; // コメント処理 // case 'S': speed = atoi(&buffer[1]); printf("speed = %d\n", speed); break; case 'W': wait = atoi(&buffer[1]); printf("wait = %d\n", wait); // Sleep(wait); break; default: n = split(buffer, p); for(i=0; i<n; i++) printf("%s : ", p[i]); printf("\n"); } } fclose(fp); }

サーボモーターの移動量コマンドを分離する

#include <stdio.h>
#include <stdlib.h>
#include <string.h>

#define MotorNum 9

void translation(char* filename);

main()
{
    translation("forward.msf");
}

int split(char buffer[],char *p[]){
    int n, m, j;
    char *q;
    p[0] = buffer;
    m=j=1;
    do {
        q = strchr(&buffer[m],',');
        if(!q) break;
        n = q - buffer;
        buffer[n]=0;
        p[j++] = &buffer[m=++n];
    } while(1);
    return j;
}

// ファイルを読み込んで解釈実行
void translation(char* filename)
{
    FILE *fp;
    char buffer[100];
    char *p[MotorNum];
    int i, n, speed, wait;

    fp = fopen(filename, "r");
    while(fscanf(fp,"%s",buffer) >0) {
        if(!strcmp(buffer,"END")) break;    // 終了 //
        switch(buffer[0]) {
            case ';':   continue;        // コメント処理 //
            case 'S':   speed = atoi(&buffer[1]);
                        printf("speed = %d\n", speed);
                        break;
            case 'W':   wait = atoi(&buffer[1]);
                        printf("wait = %d\n", wait);
                        //  Sleep(wait);
                        break;

            default:    n = split(buffer, p);
                        for(i=0; i<n; i++)
                            printf("%s : ", p[i]);
                        printf("\n");
        }
    }
    fclose(fp); 
}

１－４－８　サーボモーターの移動量をスピードを考慮に入れて求める

以下のソースをtrans6.cというファイル名で作成します。

サーボモーターの移動量をスピードを考慮に入れて求める
#include <stdio.h> #include <stdlib.h> #include <string.h> #define MotorNum 9 void translation(char* filename); main() { translation("forward.msf"); } int split(char buffer[],char p[]){ int n, m, j; char q; p[0] = buffer; m=j=1; do { q = strchr(&buffer[m],','); if(!q) break; n = q - buffer; buffer[n]=0; p[j++] = &buffer[m=++n]; } while(1); return j; } void doMove(char p[], int n, int speed) { int i, k, m; int orientation, motor, remain; for(i=0; i<n; i++) { k = atoi(p[i]+2); orientation = ((p[i]+1) == '+') ? 1: -1; motor = p[i][0]-'0'; // 手抜き // m = (k<speed) ? k : speed; printf("servo %d ", m); remain = k - m; // 残りのステップ数 // printf("残り %d\n", remain); } } // ファイルを読み込んで解釈実行 void translation(char* filename) { FILE fp; char buffer[100]; char p[MotorNum]; int i, n, speed, wait; fp = fopen(filename, "r"); while(fscanf(fp,"%s",buffer) >0) { if(!strcmp(buffer,"END")) break; // 終了// switch(buffer[0]) { case ';': continue; // コメント処理 // case 'S': speed = atoi(&buffer[1]); printf("speed = %d\n", speed); break; case 'W': wait = atoi(&buffer[1]); printf("wait = %d\n", wait); // Sleep(wait); break; default: n = split(buffer, p); for(i=0; i<n; i++) printf("%s : ", p[i]); printf("\n"); doMove(p, n, speed); } } fclose(fp); }

サーボモーターの移動量をスピードを考慮に入れて求める

#include <stdio.h>
#include <stdlib.h>
#include <string.h>

#define MotorNum 9

void translation(char* filename);

main()
{
    translation("forward.msf");
}

int split(char buffer[],char *p[]){
    int n, m, j;
    char *q;
    p[0] = buffer;
    m=j=1;
    do {
        q = strchr(&buffer[m],',');
        if(!q) break;
        n = q - buffer;
        buffer[n]=0;
        p[j++] = &buffer[m=++n];
    } while(1);
    return j;
}

void doMove(char *p[], int n, int speed)
{
    int i, k, m;
    int orientation, motor, remain;
    for(i=0; i<n; i++) {
        k = atoi(p[i]+2);
        orientation = (*(p[i]+1) == '+') ? 1: -1;
        motor = p[i][0]-'0';        // 手抜き //
        m = (k<speed) ? k : speed;
        printf("servo %d ", m);
        remain = k - m;  // 残りのステップ数 //
        printf("残り %d\n", remain);
    }
}

// ファイルを読み込んで解釈実行
void translation(char* filename)
{
    FILE *fp;
    char buffer[100];
    char *p[MotorNum];
    int i, n, speed, wait;

    fp = fopen(filename, "r");
    while(fscanf(fp,"%s",buffer) >0) {
        if(!strcmp(buffer,"END")) break;   // 終了//
        switch(buffer[0]) {
            case ';':   continue;          // コメント処理 //
            case 'S':   speed = atoi(&buffer[1]);
                        printf("speed = %d\n", speed);
                        break;
            case 'W':   wait = atoi(&buffer[1]);
                        printf("wait = %d\n", wait);
                        //  Sleep(wait);
                        break;
            default:    n = split(buffer, p);
                        for(i=0; i<n; i++)
                            printf("%s : ", p[i]);
                        printf("\n");
                        doMove(p, n, speed);
        }
    }
    fclose(fp); 
}

１－４－９　正しい移動量にする

以下のソースをtrans7.cというファイル名で作成します。

正しい移動量にする
#include <stdio.h> #include <stdlib.h> #include <string.h> #define MotorNum 9 void translation(char* filename); main() { translation("forward.msf"); } int split(char buffer[],char p[]){ int n, m, j; char q; p[0] = buffer; m=j=1; do { q = strchr(&buffer[m],','); if(!q) break; n = q - buffer; buffer[n]=0; p[j++] = &buffer[m=++n]; } while(1); return j; } void doMove(char p[], int n, int speed) { int i, j, k, m, mm; int orientation, motor, remain, value; for(i=0; i<n; i++) { k = atoi(p[i]+2); orientation = ((p[i]+1) == '+') ? 1: -1; motor = p[i][0]-'0'; // 手抜き // while(k>=speed){ printf("%d ", speed); k -= speed; } if(k != 0) printf("%d ", k); printf("\n"); } } // ファイルを読み込んで解釈実行 void translation(char* filename) { FILE fp; char buffer[100]; char p[MotorNum]; int i, n, speed, wait; fp = fopen(filename, "r"); while(fscanf(fp,"%s",buffer) >0) { if(!strcmp(buffer,"END")) break; // 終了 // switch(buffer[0]) { case ';': continue; // コメント処理 // case 'S': speed = atoi(&buffer[1]); printf("speed = %d\n", speed); break; case 'W': wait = atoi(&buffer[1]); printf("wait = %d\n", wait); // Sleep(wait); break; default: n = split(buffer, p); for(i=0; i<n; i++) printf("%s : ", p[i]); printf("\n"); doMove(p, n, speed); } } fclose(fp); }

正しい移動量にする

#include <stdio.h>
#include <stdlib.h>
#include <string.h>

#define MotorNum 9

void translation(char* filename);

main()
{
    translation("forward.msf");
}

int split(char buffer[],char *p[]){
    int n, m, j;
    char *q;
    p[0] = buffer;
    m=j=1;
    do {
        q = strchr(&buffer[m],',');
        if(!q) break;
        n = q - buffer;
        buffer[n]=0;
        p[j++] = &buffer[m=++n];
    } while(1);
    return j;
}

void doMove(char *p[], int n, int speed)
{
    int i, j, k, m, mm;
    int orientation, motor, remain, value;
    for(i=0; i<n; i++) {
        k = atoi(p[i]+2);
        orientation = (*(p[i]+1) == '+') ? 1: -1;
        motor = p[i][0]-'0';        // 手抜き //
        while(k>=speed){
            printf("%d ", speed); 
            k -= speed;
        }
        if(k != 0) printf("%d ", k); 
        printf("\n");
    }
}

// ファイルを読み込んで解釈実行
void translation(char* filename)
{
    FILE *fp;
    char buffer[100];
    char *p[MotorNum];
    int i, n, speed, wait;

    fp = fopen(filename, "r");
    while(fscanf(fp,"%s",buffer) >0) {
        if(!strcmp(buffer,"END")) break;    // 終了 //
        switch(buffer[0]) {
            case ';':   continue;        // コメント処理 //
            case 'S':   speed = atoi(&buffer[1]);
                        printf("speed = %d\n", speed);
                        break;
            case 'W':   wait = atoi(&buffer[1]);
                        printf("wait = %d\n", wait);
                        //  Sleep(wait);
                        break;
            default:    n = split(buffer, p);
                        for(i=0; i<n; i++)
                            printf("%s : ", p[i]);
                        printf("\n");
                        doMove(p, n, speed);
        }
    }
    fclose(fp); 
}

１－４－１０　サーボモーターの位置データにする

以下のソースをtrans8.cというファイル名で作成します。

サーボモーターの位置データにする
#include <stdio.h> #include <stdlib.h> #include <string.h> #define MotorNum 9 void translation(char* filename); int direction[MotorNum]; int now[MotorNum]; main() { int i; for(i=0; i<MotorNum; i++) { direction[i]=1; // 時計回りと仮定する // now[i] = 110; // 初期値は真ん中 // } translation("forward.msf"); } // カンマで区切られたデータを一つ一つに分ける // buffer: 入力データ // p[] 分けられた文字列のポインタ配列 int split(char buffer[],char p[]){ int n, m, j; char q; p[0] = buffer; m=j=1; do { q = strchr(&buffer[m],','); if(!q) break; n = q - buffer; buffer[n]=0; p[j++] = &buffer[m=++n]; } while(1); return j; } // サーボモーターを動かす void doMove(char p[], int num, int speed) { int i, j, k, n,m, mm, movement; int move; int orientation, motor, remain, value; for(i=0; i<num; i++) { k = atoi(p[i]+2); orientation = ((p[i]+1) == '+') ? 1: -1; motor = p[i][0]-'0'; // 手抜き // movement = abs(korientation-now[motor]+home[motor]); n = movement / speed; // 移動回数 // m = movement % speed; // 余りの移動ステップ // for(j=0; j<n; j++) { move = speedorientation; now[motor] += move; if(direction[motor]==1) printf("%d:%d ", speed, now[motor]); else printf("%d:%d ", speed, 220-now[motor]); } if(m != 0) { now[motor] = home[motor]+korientation; if(direction[motor]==1) printf("%d:%d ", speed, now[motor]); else printf("%d:%d ", speed, 220-now[motor]); } } } // ファイルを読み込んで解釈実行 void translation(char* filename) { FILE fp; char buffer[100]; char p[MotorNum]; int i, n, speed, wait; fp = fopen(filename, "r"); while(fscanf(fp,"%s",buffer) >0) { if(!strcmp(buffer,"END")) break; // 終了 // switch(buffer[0]) { case ';': continue; // コメント処理 // case 'S': speed = atoi(&buffer[1]); printf("speed = %d\n", speed); break; case 'W': wait = atoi(&buffer[1]); printf("wait = %d\n", wait); // Sleep(wait); break; default: n = split(buffer, p); for(i=0; i<n; i++) printf("%s : ", p[i]); printf("\n"); doMove(p, n, speed); } } fclose(fp); }

サーボモーターの位置データにする

#include <stdio.h>
#include <stdlib.h>
#include <string.h>

#define MotorNum 9

void translation(char* filename);

int direction[MotorNum];
int now[MotorNum];

main()
{

    int i;
    for(i=0; i<MotorNum; i++) {
        direction[i]=1;     // 時計回りと仮定する //
        now[i] = 110;       // 初期値は真ん中 //
    }

    translation("forward.msf");
}

// カンマで区切られたデータを一つ一つに分ける
// buffer: 入力データ
// *p[] 分けられた文字列のポインタ配列
int split(char buffer[],char *p[]){
    int n, m, j;
    char *q;
    p[0] = buffer;
    m=j=1;
    do {
        q = strchr(&buffer[m],',');
        if(!q) break;
        n = q - buffer;
        buffer[n]=0;
        p[j++] = &buffer[m=++n];
    } while(1);
    return j;
}

// サーボモーターを動かす

void doMove(char *p[], int num, int speed)
{
    int i, j, k, n,m, mm, movement;
    int move;
    int orientation, motor, remain, value;
    for(i=0; i<num; i++) {
        k = atoi(p[i]+2);
        orientation = (*(p[i]+1) == '+') ? 1: -1;
        motor = p[i][0]-'0';        // 手抜き //
        movement = abs(k*orientation-now[motor]+home[motor]);
        n = movement / speed;       // 移動回数 //
        m = movement % speed;       // 余りの移動ステップ //
        for(j=0; j<n; j++) {
             move = speed*orientation;
             now[motor] += move;
             if(direction[motor]==1) printf("%d:%d ", speed, now[motor]);
             else printf("%d:%d ", speed, 220-now[motor]); 
        }
        if(m != 0) {
             now[motor] = home[motor]+k*orientation;
             if(direction[motor]==1) printf("%d:%d ", speed, now[motor]);
             else printf("%d:%d ", speed, 220-now[motor]); 
        }
    }
}


// ファイルを読み込んで解釈実行
void translation(char* filename)
{
    FILE *fp;
    char buffer[100];
    char *p[MotorNum];
    int i, n, speed, wait;

    fp = fopen(filename, "r");
    while(fscanf(fp,"%s",buffer) >0) {
        if(!strcmp(buffer,"END")) break;    // 終了 //
        switch(buffer[0]) {
            case ';':   continue;        // コメント処理 //
            case 'S':   speed = atoi(&buffer[1]);
                        printf("speed = %d\n", speed);
                        break;
            case 'W':   wait = atoi(&buffer[1]);
                        printf("wait = %d\n", wait);
                        //  Sleep(wait);
                        break;
            default:    n = split(buffer, p);
                        for(i=0; i<n; i++)
                            printf("%s : ", p[i]);
                        printf("\n");
                        doMove(p, n, speed);
        }
    }
    fclose(fp); 
}

１－４－１１　実際に動く形にする

以下のソースをtrans9.cというファイル名で作成します。

実際に動く形にする
#include <stdio.h> #include "comm.h" #define MotorNum 9 void translation(char* filename); int direction[MotorNum]; int now[MotorNum]; main() { int i; for(i=0; i<MotorNum; i++) { direction[i]=1; // 時計回りと仮定する // now[i] = 110; // 初期値は真ん中 // } if(initServo("COM5")) return -1; translation("forward.msf"); } // カンマで区切られたデータを一つ一つに分ける // buffer: 入力データ // p[] 分けられた文字列のポインタ配列 int split(char buffer[],char p[]){ int n, m, j; char q; p[0] = buffer; m=j=1; do { q = strchr(&buffer[m],','); if(!q) break; n = q - buffer; buffer[n]=0; p[j++] = &buffer[m=++n]; } while(1); return j; } void doMove(char p[], int num, int speed) { int i, j, k, n,m, mm, movement; int move; int orientation, motor, remain, value; for(i=0; i<num; i++) { k = atoi(p[i]+2); orientation = ((p[i]+1) == '+') ? 1: -1; motor = p[i][0]-'0'; // 手抜き // movement = abs(korientation-now[motor]+home[motor]); n = movement / speed; // 移動回数 // m = movement % speed; // 余りの移動ステップ // for(j=0; j<n; j++) { move = speedorientationdirection[motor]; now[motor] += move; if(direction[motor]==1) servo(motor, now[motor]); else servo(motor, 220-now[motor]); Sleep(10); } if(m != 0) { now[motor] = home[motor]+korientation; if(direction[motor]==1) servo(motor, now[motor]); else servo(motor, 220-now[motor]); Sleep(10); } } } // ファイルを読み込んで解釈実行 void translation(char filename) { FILE fp; char buffer[100]; char p[MotorNum]; int i, n, speed, wait; fp = fopen(filename, "r"); while(fscanf(fp,"%s",buffer) >0) { if(!strcmp(buffer,"END")) break; // 終了 // switch(buffer[0]) { case ';': continue; // コメント処理 // case 'S': speed = atoi(&buffer[1]); printf("speed = %d\n", speed); break; case 'W': wait = atoi(&buffer[1]); printf("wait = %d\n", wait); Sleep(wait); break; default: n = split(buffer, p); for(i=0; i<n; i++) printf("%s : ", p[i]); printf("\n"); doMove(p, n, speed); } } fclose(fp); }

実際に動く形にする

#include <stdio.h>

#include "comm.h"

#define MotorNum 9

void translation(char* filename);

int direction[MotorNum];
int now[MotorNum];

main()
{
    int i;
    for(i=0; i<MotorNum; i++) {
        direction[i]=1;     // 時計回りと仮定する //
        now[i] = 110;       // 初期値は真ん中 //
    }

    if(initServo("COM5")) return -1;

    translation("forward.msf");
}

// カンマで区切られたデータを一つ一つに分ける
// buffer: 入力データ
// *p[] 分けられた文字列のポインタ配列
int split(char buffer[],char *p[]){
    int n, m, j;
    char *q;
    p[0] = buffer;
    m=j=1;
    do {
        q = strchr(&buffer[m],',');
        if(!q) break;
        n = q - buffer;
        buffer[n]=0;
        p[j++] = &buffer[m=++n];
    } while(1);
    return j;
}

void doMove(char *p[], int num, int speed)
{
    int i, j, k, n,m, mm, movement;
    int move;
    int orientation, motor, remain, value;
    for(i=0; i<num; i++) {
        k = atoi(p[i]+2);
        orientation = (*(p[i]+1) == '+') ? 1: -1;
        motor = p[i][0]-'0';        // 手抜き //
        movement = abs(k*orientation-now[motor]+home[motor]);
        n = movement / speed;       // 移動回数 //
        m = movement % speed;       // 余りの移動ステップ //
        for(j=0; j<n; j++) {
             move = speed*orientation*direction[motor];
             now[motor] += move;
             if(direction[motor]==1) servo(motor, now[motor]);
             else servo(motor, 220-now[motor]); 
             Sleep(10);
       }
        if(m != 0) {
             now[motor] = home[motor]+k*orientation;
             if(direction[motor]==1) servo(motor, now[motor]);
             else servo(motor, 220-now[motor]); 
             Sleep(10);
        }
    }
}


// ファイルを読み込んで解釈実行
void translation(char* filename)
{
    FILE *fp;
    char buffer[100];
    char *p[MotorNum];
    int i, n, speed, wait;

    fp = fopen(filename, "r");
    while(fscanf(fp,"%s",buffer) >0) {
        if(!strcmp(buffer,"END")) break;    // 終了 //
        switch(buffer[0]) {
            case ';':   continue;        // コメント処理 //
            case 'S':   speed = atoi(&buffer[1]);
                        printf("speed = %d\n", speed);
                        break;
            case 'W':   wait = atoi(&buffer[1]);
                        printf("wait = %d\n", wait);
                        Sleep(wait);
                        break;
            default:    n = split(buffer, p);
                        for(i=0; i<n; i++)
                            printf("%s : ", p[i]);
                        printf("\n");
                        doMove(p, n, speed);
        }
    }
    fclose(fp); 
}

課題

上記read.cのソースファイルとtrans9.cを合体させpuchi.cとして作り、main関数を正しく修正しロボットを動かしなさい。

１－５　18個のサーボモーターをコントロールする

１－５－１　動作チェック

以下のソースをservo2.cというファイル名で作成します。

動作チェック
// プチロボットのサーボモーターのテストプログラム // 二つのポートを使ってコントロールする（全部で18個のサーボモータ） // g++ でコンパイルする #include #include #include "comm.h" int main(int argc, char** argv) { int key, end=1; int position=110; int i, save; char *aux[5] = {"COM3", "COM5"}; int err=0; if(argc == 2) aux[0][3]=argv[1][0]; else if(argc ==3) aux[1][3]=argv[1][0]; if(initServo(aux, 0)) err=1; if(initServo(aux, 1)) err+=2; if(err) printf("COMポートがおかしい\n"); if(err==3) { printf("COMポートが二つとも使えない\n"); return -1; } printf("プチロボットテストプログラム\n" "+:10 -:-10 space:1 other:-1 q:終了\n"); while(end) { if(kbhit()) { key=readkey(); save = position; switch(key&0x7f){ case '+': case ';': position+=10; break; case '-': position-=10; break; case 'q': end=0; break; case ' ': position++; break; default: position--; } if((position <1) \|\| (220<position)) position = save; else { printf("%d\n", position); fflush(stdout); } } for(i=0; i<9; i++) { if(!(err & 1)) servo(i, position, 0); if(!(err & 2)) servo(i, position, 1); } } if(!(err & 1)) closeServo(0); if(!(err & 2)) closeServo(1); }

動作チェック

// プチロボットのサーボモーターのテストプログラム 
// 二つのポートを使ってコントロールする（全部で18個のサーボモータ）
// g++ でコンパイルする

#include 
#include 
#include "comm.h" 

int main(int argc, char** argv)
    {
    int key, end=1;
    int position=110;
    int i, save;
    char *aux[5] = {"COM3", "COM5"};
    int err=0;
    
    if(argc == 2) aux[0][3]=argv[1][0];
    else if(argc ==3) aux[1][3]=argv[1][0];
    if(initServo(aux, 0)) err=1;
    if(initServo(aux, 1)) err+=2;
    if(err) printf("COMポートがおかしい\n");
    if(err==3) {
        printf("COMポートが二つとも使えない\n");
        return -1;
    }
    printf("プチロボットテストプログラム\n"
           "+:10 -:-10 space:1 other:-1 q:終了\n");
    while(end) {
        if(kbhit()) {
            key=readkey();
            save = position;
            switch(key&0x7f){
                case '+': 
                case ';': position+=10; break;
                case '-': position-=10; break;
                case 'q': end=0; break;
                case ' ': position++; break;
                default:  position--;
            }
            if((position <1) || (220<position)) position = save;
            else {
                printf("%d\n", position);
                fflush(stdout);
            }
        }
        for(i=0; i<9; i++) {
           if(!(err & 1)) servo(i, position, 0);
           if(!(err & 2)) servo(i, position, 1);
        }
    }
    if(!(err & 1)) closeServo(0);
    if(!(err & 2)) closeServo(1);
}

課題

puchi.cを元ととし、servo2.cを参考に修正し18個のサーボモーターをコントロールするpuchi2.cを作りなさい。
この時、Puchi_Robo_Config.ini及びmsfファイルを修正する必要があります。

Puchi_Robo_Config.iniについて

{Comm]
追加のサーボモーターを動かすポートはPort2=2というようにする。
[Direction]と[HomePosition]
モーターを意味するMの次の数字の代わりに追加のモーターの場合は16進数と似た考えでABC…とします。

0 1 2 3 4 5 6 7 8

A B C D E F G H I

msfファイルについて

Puchi_Robo_Config.iniの[Direction]と同様にモーター番号はABC…とします。

Lesson1 プチロボをC言語で動かす

１－１ Dev-Cppをセットアップする

１－２ サーボモーターを動かしてみる

１－３ プチロボの動きを作る

１－４ msfファイルを読む

１－４－１ msfファイルについて

１－４－２ 初期設定ファイルを読んでサーボモーターをホームポジションに移動させる

１－４－３ msfファイルを読んでみる

１－４－４ スピードコマンドを解析する

１－４－５ waitコマンドを解析する

１－４－６ スイッチ文に書き換える

１－４－７ サーボモーターの移動量コマンドを分離する

１－４－８ サーボモーターの移動量をスピードを考慮に入れて求める

１－４－９ 正しい移動量にする

１－４－１０ サーボモーターの位置データにする

１－４－１１ 実際に動く形にする

１－５ 18個のサーボモーターをコントロールする

１－５－１ 動作チェック