C言語でEV3開発(13)
opOUTPUT_STEP_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 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_SPEEDコマンドを実行する。                        */
/*                                                                           */
/*===========================================================================*/
int StepSpeed(
        UBYTE ch,
        UBYTE speed,
        UWORD step1,
        UWORD step2,
        UWORD step3,
        UBYTE brake) {
    int ret;
    size_t size;
    STEPSPEED stepSpeed = { 0 };
    stepSpeed.Cmd = opOUTPUT_STEP_SPEED;
    stepSpeed.Nos = ch;
    stepSpeed.Speed = speed;
    stepSpeed.Step1 = step1;
    stepSpeed.Step2 = step2;
    stepSpeed.Step3 = step3;
    stepSpeed.Brake = brake;
    size = sizeof(stepSpeed);
    ret = write(motor_fp, (void *)&stepSpeed, size);

    return ret;
}

/*****************************************************************************/
/*                                                                           */
/*  メイン関数                                                               */
/*                                                                           */
/*****************************************************************************/
int main() {
    UBYTE speed = 100;
    UWORD step1 = 720;
    UWORD step2 = 3600;
    UWORD step3 = 720;
    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);
    //	opOUTPUT_STARTコマンドは、実効しない。
    //MotorStart(CH_D);
    StepSpeed(CH_D, speed, step1, step2, step3, brake);
    sleep(5);

    MotorStop(CH_D);

    ProgramStop();

    close(motor_fp);
    exit(0);
}