C言語でEV3開発(12)
opOUTPUT_TIME_POWERコマンド

#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 MotorReset(UBYTE ch) {
    UBYTE Buf[4];
    int ret;

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

    return ret;
}

/*===========================================================================*/
/*                                                                           */
/*  今回対象のopOUTPUT_TIME_POWERコマンドを実行する                          */
/*                                                                           */
/*===========================================================================*/
int TimePower(
        UBYTE ch,
        UBYTE power,
        UWORD time1,
        UWORD time2,
        UWORD time3,
        UBYTE brake)
{
    int ret;
    size_t size;
    TIMEPOWER timePower = { 0 };

    timePower.Cmd = opOUTPUT_TIME_POWER;
    timePower.Nos = (DATA8) ch;
    timePower.Power = (DATA8) power;
    timePower.Time1 = time1;
    timePower.Time2 = time2;
    timePower.Time3 = time3;
    timePower.Brake = brake;
    size = sizeof(timePower);
    ret = write(motor_fp, (void *) &timePower, size);

    return ret;
}

/*****************************************************************************/
/*                                                                           */
/*  メイン関数                                                               */
/*                                                                           */
/*****************************************************************************/
int main() {
    UBYTE power = 100;
    UWORD time1 = 3000;
    UWORD time2 = 3000;
    UWORD time3 = 0;
    UBYTE brake = 1;

    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);
    MotorStart(CH_D);

    TimePower(CH_D, power, time1, time2, time3, brake);
    sleep(15);

    MotorStop(CH_D);

    ProgramStop();

    close(motor_fp);
    exit(0);
}