C言語でEV3開発(9)-opOUTPUT_SPEEDコマンド

2015年4月30日2021年1月30日

どもです。
今回のエントリーでは、前回に引き続きEV3のモーターを動かしていきます。
ただし、単純にモーターを動かすのではなく、EV3のファームウェアで用意されている「コマンド」を使用して動かしてみます。

1.コマンドとは？

「とは？」と書いたものの、説明するまでもなく、そのまま「命令」です。

具体的には、前回のエントリで使用した「opPROGRAM_START」や「opOUTPUT_START」が、それにあたります。
EV3でモーターを動かす際には、この「コマンド」を使用します。
これまた前回のエントリーで紹介した「opOUTPUT_POWER」が、具体的な「モーターを動かすためのコマンド」になります。
EV3では、これらのコマンドが様々用意されております。

2. opOUTPUT_SPEEDコマンド

今回紹介するコマンドです。
前回のエントリーで使用していたコマンドです。
実際にコマンドデータを実行する場合には、以下のようにデータを設定します。

UBYTE Buf[4];
Buf[0] = opOUTPUT_SPEED;
Buf[1] = ch;
Buf[2] = power; //  -100～100

あとは、これをwrite関数に書き込めばモーターが動作します。
(ただし、あらかじめopPROGRAM_START/opOUTPUT_STARTコマンドを実行しておく必要があります。)
なお、powerにマイナスの値を設定した場合には、回転方向が逆になります。

3.動かしてみた

というわけで、opOUTPUT_SPEEDコマンドを使用してモーターを動作させるためのサンプルプログラムを載せます。

#include <stdio.h>
#include <fcntl.h>
#include <stdlib.h>
#include <unistd.h>
#include <string.h>
#include <signal.h>
#include "lms2012.h"

#define CH_A    (0x01)
#define CH_B    (0x02)
#define CH_C    (0x04)
#define CH_D    (0x08)

//   モーターのポート名
#define PWM_MOTOR_PORT_NAME ("/dev/lms_pwm")

typedef unsigned int UINT;
typedef signed int SINT;
//typedef unsigned char UBYTE;
//typedef signed int SBYTE;
typedef void VOID;

//  モーターのファイルディスクリプター
SINT motor_fp;

int ProgramStart(void) {
    UBYTE Buf[4];
    int ret;

    Buf[0] = opPROGRAM_START;
    ret = write(motor_fp, &Buf[0], 1);

    return ret;
}

int ProgramStop(void) {
    UBYTE Buf[4];
    int ret;

    Buf[0] = opPROGRAM_STOP;
    ret = write(motor_fp, &Buf[0], 1);

    return ret;
}

int MotorStart(UBYTE ch) {
    UBYTE Buf[4];
    int ret;

    Buf[0] = opOUTPUT_START;
    Buf[1] = ch;
    ret = write(motor_fp, &Buf[0], 2);

    return ret;
}

int MotorStop(UBYTE ch) {
    UBYTE Buf[4];
    int ret;

    Buf[0] = opOUTPUT_STOP;
    Buf[1] = ch;
    ret = write(motor_fp, &Buf[0], 2);

    return ret;
}

int MotorSpeed(UBYTE ch, UBYTE speed)
{
    UBYTE Buf[4];
    int ret;

    Buf[0] = opOUTPUT_SPEED;
    Buf[1] = ch;
    Buf[2] = speed;
    printf("0x%02x 0x%02x 0x%02x\n", Buf[0], Buf[1], Buf[2]);
    ret = write(motor_fp, Buf, 3);

    return ret;
}

int MotorReset(UBYTE ch) {
    UBYTE Buf[4];
    int ret;

    Buf[0] = opOUTPUT_RESET;
    Buf[1] = ch;
    ret = write(motor_fp, Buf, 2);

    return ret;
}

int main() {
    motor_fp = open(PWM_MOTOR_PORT_NAME, O_RDWR);
    if (motor_fp < 0) {
        printf("Can not open MOTOR PORT\r\n");

        return (-1);
    }

    ProgramStart();
    ProgramStop();
    ProgramStart();

    MotorReset(CH_D);
    MotorSpeed(CH_D, 30);
    MotorStart(CH_D);
    sleep(3);
    MotorSpeed(CH_D, 60);
    sleep(3);
    MotorSpeed(CH_D, 100);
    sleep(3);
    MotorSpeed(CH_D, 60);
    sleep(3);
    MotorSpeed(CH_D, 30);
    sleep(3);
    MotorSpeed(CH_D, 0);
    sleep(3);
    MotorSpeed(CH_D, (UBYTE)-30);
    sleep(3);
    MotorSpeed(CH_D, (UBYTE)-60);
    sleep(3);
    MotorSpeed(CH_D, (UBYTE)-100);
    sleep(3);
    MotorSpeed(CH_D, (UBYTE)-60);
    sleep(3);
    MotorSpeed(CH_D, (UBYTE)-30);
    sleep(3);
    MotorSpeed(CH_D, 0);
    sleep(3);
    MotorStop(CH_D);

    ProgramStop();

    close(motor_fp);
    exit(0);
}

上記のコードですが、ほとんどの部分は前回のエントリーから変更はありません。
変更箇所は、コマンドの実行手順です。

MotorSpeed(CH_D, 30);
MotorStart(CH_D);

L.106～L.107です。
前回は、実行順序は逆でした。つまり、「opOUTPUT_START」→「opOUTPUT_SPEED」の順番で実行していました。
しかし、今回は「opOUTPUT_SPEED」→「opOUTPUT_START」の順番で実行しています。
現在、原因は不明ですが、この順番で実行しないとモーターが動作しません。

というわけで、実際に動かしてみた場合の動画です。