C言語でEV3開発(11)
opOUTPUT_STEP_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_STEP_POWERコマンドを実行する                          */
/*                                                                           */
/*===========================================================================*/
int StepPower(
        UBYTE ch,
        SBYTE power,
        UBYTE step1,
        UBYTE step2,
        UBYTE step3,
        UBYTE brake) {
    int ret;
    size_t size;
    STEPPOWER stepPower = { 0 };

    stepPower.Cmd = opOUTPUT_STEP_POWER;
    stepPower.Nos = (DATA8) ch;
    stepPower.Power = (DATA8) power;
    stepPower.Step1 = step1;
    stepPower.Step2 = step2;
    stepPower.Step3 = step3;
    stepPower.Brake = brake;
    size = sizeof(stepPower);
    ret = write(motor_fp, (void *) &stepPower, size);

    return ret;
}

/*****************************************************************************/
/*                                                                           */
/*  メイン関数                                                               */
/*                                                                           */
/*****************************************************************************/
int main() {
    UBYTE power = 100;
    UBYTE step1 = 255;
    UBYTE step2 = 255;
    UBYTE step3 = 255;
    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);

    StepPower(CH_D, power, step1, step2, step3, brake);
    sleep(3);

    MotorStop(CH_D);

    ProgramStop();

    close(motor_fp);
    exit(0);
}