More than 3 years have passed since last update.

日記；回すだけIII ⑭ C++でプログラミング＜その3 モータを回すプログラム（EPOS4）＞ベータ版

Last updated at 2022-04-07Posted at 2022-04-01

　前回、Visual C++を使って、オブジェクト・ディクショナリの一部を読み出しました。ここでは、Profile Position Modeでモータを回転します。

ソースcppの追加

　画面左のソリューションエクスプローラのソースファイルの上で右クリックします。
　C++ファイル（.cpp）を選択して追加します。
　もともとあったepos.cppは、削除-除外をします。

　回転部分はmove()関数にしました。VCS_MoveToPosition()は指定した位置まで動かしますが、速度の指定はできないので、Profile_velocity（0x6081）に直接書き込んでいます。うまく動いているように見えるのですが。
　台形駆動で目標位置まで動かします。

（2022/04/07　図を修正しました）

　メイン部分では、CWで10000、CCWで50000移動させています。

#include <iostream>
#include "Definitions.h"

HANDLE keyHandle = 0;
char* deviceName = (char*)"EPOS4";
char* protocolStackName = (char*)"CANopen";
char* interfaceName = (char*)"IXXAT_USB-to-CAN V2 compact 0";
char* portName = (char*)"CAN0";

DWORD errorCode = 0;
DWORD timeout = 0;
WORD nodeId = 5;

WORD State = 0;
DWORD ProfileVelocity = 1000;
DWORD ProfileAcceleration = 1000;
DWORD ProfileDeceleration = 1000;
long PositionIs = 0;
long TargetPosition = 0;

DWORD move(DWORD pTargetPosition, DWORD pProfileVelocity)
{
    BOOL Absolute = 0;
    BOOL Immediately = TRUE;
    VCS_SetDisableState(keyHandle, nodeId, &errorCode);
    VCS_SetEnableState(keyHandle, nodeId, &errorCode);
    VCS_MoveToPosition(keyHandle, nodeId, pTargetPosition, Absolute, Immediately, &errorCode);
    void* Profile_velocity;
    DWORD NbOfBytesWritten = 0;
    VCS_SetObject(keyHandle, nodeId, 0x6081, 0x00, &Profile_velocity, 4, &NbOfBytesWritten, &errorCode);
    return errorCode;
}

int main()
{
    printf("start EPOS4\n");

    keyHandle = VCS_OpenDevice(deviceName, protocolStackName, interfaceName, portName, &errorCode);

    if ((BOOL)keyHandle) {

        // enable_state
        VCS_ClearFault(keyHandle, nodeId, &errorCode);
        VCS_SetEnableState(keyHandle, nodeId, &errorCode);

        // Initialisation
        VCS_SetDisableState(keyHandle, nodeId, &errorCode);
        VCS_SetOperationMode(keyHandle, nodeId, 1, &errorCode);
        ProfileVelocity = 2500;
        ProfileAcceleration = 200;
        ProfileDeceleration = 200;
        VCS_SetPositionProfile(keyHandle, nodeId, ProfileVelocity, ProfileAcceleration, ProfileDeceleration, &errorCode);

        VCS_GetPositionIs(keyHandle, nodeId, &PositionIs, &errorCode);
        printf("\norg positionIs--- %ld\n", PositionIs);

        //move

        move(10000, 50);

        VCS_GetTargetPosition(keyHandle, nodeId, &TargetPosition, &errorCode);
        printf("\nTargetPosition--- %ld\n", TargetPosition);
        Sleep(3000);

        VCS_GetPositionIs(keyHandle, nodeId, &PositionIs, &errorCode);
        printf("\nnew positionIs--- %ld\n", PositionIs);

        move(-50000, 2000);

        VCS_GetTargetPosition(keyHandle, nodeId, &TargetPosition, &errorCode);
        printf("\nTargetPosition--- %ld\n", TargetPosition);
        Sleep(3000);

        VCS_GetPositionIs(keyHandle, nodeId, &PositionIs, &errorCode);
        printf("\nnew positionIs--- %ld\n", PositionIs);
        Sleep(3000);

        printf("\nReset state");
        VCS_ResetDevice(keyHandle, nodeId, &errorCode);
        VCS_SetDisableState(keyHandle, nodeId, &errorCode);
    }
    VCS_CloseDevice(keyHandle, &errorCode);
}

　実行した様子です。

実際のデータをWiresharkで見る

　Wiresharkを立ちあげてUSBのパケットだけを見ます。キャプチャを開始し、上記のプログラムを実行します。キャプチャを終了し、Infoをクリックしてソートします。その中で、URB_BULK outに注目します。Sourceがhostをクリックして、Leftover Capture Data：部分をコピペし、メモ帳に張り付けていきます。真ん中より後半に4160があります。0x6041のStatusword（Rx）です。

Leftover Capture Data: 1800000000050600000000088b010000004041600000000000
Leftover Capture Data: 1800000000050600000000088b010000004010200000000000
Leftover Capture Data: 1800000000050600000000088b010000002203100000000000
Leftover Capture Data: 1800000000050600000000088b010000004041600000000000
Leftover Capture Data: 1800000000050600000000088b010000002240600006000000
Leftover Capture Data: 1800000000050600000000088b010000004041600000000000
Leftover Capture Data: 1800000000050600000000088b010000004041600000000000
Leftover Capture Data: 1800000000050600000000088b01000000224060000f000000
Leftover Capture Data: 1800000000050600000000088b010000004041600000000000
Leftover Capture Data: 1800000000050600000000088b010000004051310000000000
Leftover Capture Data: 1800000000050600000000088b010000004051310100000000
Leftover Capture Data: 1800000000050600000000088b010000004051310200000000
Leftover Capture Data: 1800000000050600000000088b010000004051310300000000
Leftover Capture Data: 1800000000050600000000088b010000004041600000000000
Leftover Capture Data: 1800000000050600000000088b010000004051310000000000
Leftover Capture Data: 1800000000050600000000088b010000004051310100000000
Leftover Capture Data: 1800000000050600000000088b010000004051310200000000
Leftover Capture Data: 1800000000050600000000088b010000004051310300000000
Leftover Capture Data: 1800000000050600000000088b010000004041600000000000
Leftover Capture Data: 1800000000050600000000088b010000004041600000000000
Leftover Capture Data: 1800000000050600000000088b010000004041600000000000
Leftover Capture Data: 1800000000050600000000088b010000002240600000000000
Leftover Capture Data: 1800000000050600000000088b010000004041600000000000
Leftover Capture Data: 1800000000050600000000088b01000000405b600000000000
Leftover Capture Data: 1800000000050600000000088b0100000040e0300300000000
Leftover Capture Data: 1800000000050600000000088b010000004051310000000000
Leftover Capture Data: 1800000000050600000000088b010000004051310100000000
Leftover Capture Data: 1800000000050600000000088b010000004051310200000000
Leftover Capture Data: 1800000000050600000000088b010000004051310300000000
Leftover Capture Data: 1800000000050600000000088b010000004041600000000000
Leftover Capture Data: 1800000000050600000000088b010000004041600000000000
Leftover Capture Data: 1800000000050600000000088b010000002260600001000000
Leftover Capture Data: 1800000000050600000000088b0100000022816000c4090000
Leftover Capture Data: 1800000000050600000000088b0100000022836000c8000000
Leftover Capture Data: 1800000000050600000000088b0100000022846000c8000000
Leftover Capture Data: 1800000000050600000000088b010000004064600000000000
Leftover Capture Data: 1800000000050600000000088b010000004041600000000000
Leftover Capture Data: 1800000000050600000000088b010000004041600000000000
Leftover Capture Data: 1800000000050600000000088b010000002240600006000000
Leftover Capture Data: 1800000000050600000000088b010000004041600000000000
Leftover Capture Data: 1800000000050600000000088b010000004041600000000000
Leftover Capture Data: 1800000000050600000000088b01000000224060000f000000
Leftover Capture Data: 1800000000050600000000088b010000004041600000000000
Leftover Capture Data: 1800000000050600000000088b010000004051310000000000
Leftover Capture Data: 1800000000050600000000088b010000004051310100000000
Leftover Capture Data: 1800000000050600000000088b010000004051310200000000
Leftover Capture Data: 1800000000050600000000088b010000004051310300000000
Leftover Capture Data: 1800000000050600000000088b010000004041600000000000
Leftover Capture Data: 1800000000050600000000088b010000004051310000000000
Leftover Capture Data: 1800000000050600000000088b010000004051310100000000
Leftover Capture Data: 1800000000050600000000088b010000004051310200000000
Leftover Capture Data: 1800000000050600000000088b010000004051310300000000
Leftover Capture Data: 1800000000050600000000088b010000004041600000000000
Leftover Capture Data: 1800000000050600000000088b010000004041600000000000
Leftover Capture Data: 1800000000050600000000088b01000000227a600010270000
Leftover Capture Data: 1800000000050600000000088b010000004041600000000000
Leftover Capture Data: 1800000000050600000000088b010000004040600000000000
Leftover Capture Data: 1800000000050600000000088b01000000224060007f000000
Leftover Capture Data: 1800000000050600000000088b010000004041600000000000
Leftover Capture Data: 1800000000050600000000088b01000000224060000f000000
Leftover Capture Data: 1800000000050600000000088b0100000022816000cccccccc
Leftover Capture Data: 1800000000050600000000088b01000000407a600000000000
Leftover Capture Data: 1800000000050600000000088b010000004064600000000000
Leftover Capture Data: 1800000000050600000000088b010000004041600000000000
Leftover Capture Data: 1800000000050600000000088b010000002240600000000000
Leftover Capture Data: 1800000000050600000000088b010000004041600000000000
Leftover Capture Data: 1800000000050600000000088b01000000405b600000000000
Leftover Capture Data: 1800000000050600000000088b0100000040e0300300000000
Leftover Capture Data: 1800000000050600000000088b010000004051310000000000
Leftover Capture Data: 1800000000050600000000088b010000004051310100000000
Leftover Capture Data: 1800000000050600000000088b010000004051310200000000
Leftover Capture Data: 1800000000050600000000088b010000004051310300000000
Leftover Capture Data: 1800000000050600000000088b010000004041600000000000
Leftover Capture Data: 1800000000050600000000088b010000004041600000000000
Leftover Capture Data: 1800000000050600000000088b010000004041600000000000
Leftover Capture Data: 1800000000050600000000088b010000002240600006000000
Leftover Capture Data: 1800000000050600000000088b010000004041600000000000
Leftover Capture Data: 1800000000050600000000088b010000004041600000000000
Leftover Capture Data: 1800000000050600000000088b01000000224060000f000000
Leftover Capture Data: 1800000000050600000000088b010000004041600000000000
Leftover Capture Data: 1800000000050600000000088b010000004051310000000000
Leftover Capture Data: 1800000000050600000000088b010000004051310100000000
Leftover Capture Data: 1800000000050600000000088b010000004051310200000000
Leftover Capture Data: 1800000000050600000000088b010000004051310300000000
Leftover Capture Data: 1800000000050600000000088b010000004041600000000000
Leftover Capture Data: 1800000000050600000000088b010000004051310000000000
Leftover Capture Data: 1800000000050600000000088b010000004051310100000000
Leftover Capture Data: 1800000000050600000000088b010000004051310200000000
Leftover Capture Data: 1800000000050600000000088b010000004051310300000000
Leftover Capture Data: 1800000000050600000000088b010000004041600000000000
Leftover Capture Data: 1800000000050600000000088b010000004041600000000000
Leftover Capture Data: 1800000000050600000000088b01000000227a6000b03cffff
Leftover Capture Data: 1800000000050600000000088b010000004041600000000000
Leftover Capture Data: 1800000000050600000000088b010000004040600000000000
Leftover Capture Data: 1800000000050600000000088b01000000224060007f000000
Leftover Capture Data: 1800000000050600000000088b010000004041600000000000
Leftover Capture Data: 1800000000050600000000088b010000004041600000000000
Leftover Capture Data: 1800000000050600000000088b01000000224060000f000000
Leftover Capture Data: 1800000000050600000000088b0100000022816000cccccccc
Leftover Capture Data: 1800000000050600000000088b01000000407a600000000000
Leftover Capture Data: 1800000000050600000000088b010000004064600000000000
Leftover Capture Data: 1800000000050600000000088b0100000022511f0102000000
Leftover Capture Data: 1800000000050600000000088b010000004041600000000000
Leftover Capture Data: 1800000000050600000000088b010000002240600000000000
Leftover Capture Data: 1800000000050600000000088b010000004041600000000000
Leftover Capture Data: 1800000000050600000000088b01000000405b600000000000
Leftover Capture Data: 1800000000050600000000088b0100000040e0300300000000
Leftover Capture Data: 1800000000050600000000088b010000004051310000000000
Leftover Capture Data: 1800000000050600000000088b010000004051310100000000
Leftover Capture Data: 1800000000050600000000088b010000004051310200000000
Leftover Capture Data: 1800000000050600000000088b010000004051310300000000
Leftover Capture Data: 1800000000050600000000088b010000004041600000000000
Leftover Capture Data: 1800000000050600000000088b010000004041600000000000

　必要だと思われる部分を残し、ハンド・デコードしました。想定した出力になっているようです。しかし、速度の情報がおかしなデータになっているので、値の渡し方が間違っているかもしれません。

40 4160 00 0000 0000
40 1020 00 0000 0000
22 0310 00 0000 0000
40 4160 00 0000 0000
22 4060 00 0600 0000  Controlword (Shutdown)
40 4160 00 0000 0000
40 4160 00 0000 0000
22 4060 00 0f00 0000  Controlword (Switch-on)
40 4160 00 0000 0000
40 4160 00 0000 0000
40 4160 00 0000 0000
40 4160 00 0000 0000
40 4160 00 0000 0000
22 4060 00 0000 0000
40 4160 00 0000 0000
40 5b60 00 0000 0000
40 e030 03 0000 0000
40 4160 00 0000 0000
40 4160 00 0000 0000
22 6060 00 0100 0000      1 Modes of Operation
22 8160 00 c409 0000   2500 Profile Velocity
22 8360 00 c800 0000    200 Profile Acceleration 
22 8460 00 c800 0000    200 Profile Deceleration
40 6460 00 0000 0000
40 4160 00 0000 0000
40 4160 00 0000 0000
22 4060 00 0600 0000  Controlword (Shutdown)
40 4160 00 0000 0000
40 4160 00 0000 0000
22 4060 00 0f00 0000  Controlword (Switch-on)
40 4160 00 0000 0000
40 4160 00 0000 0000
40 4160 00 0000 0000
40 4160 00 0000 0000
22 7a60 00 1027 0000  10000 Target Position
40 4160 00 0000 0000
40 4060 00 0000 0000
22 4060 00 7f00 0000  Controlword (relative pos., start immediately)
40 4160 00 0000 0000
22 4060 00 0f00 0000  Controlword (Switch-on)
22 8160 00 cccc cccc  Profile velocity 
40 7a60 00 0000 0000　0 Target Position
40 6460 00 0000 0000  Position actual value
40 4160 00 0000 0000
22 4060 00 0000 0000
40 4160 00 0000 0000
40 5b60 00 0000 0000
40 e030 03 0000 0000
40 4160 00 0000 0000
40 4160 00 0000 0000
40 4160 00 0000 0000
22 4060 00 0600 0000  Controlword (Shutdown)
40 4160 00 0000 0000
40 4160 00 0000 0000
22 4060 00 0f00 0000  Controlword (Switch-on)
40 4160 00 0000 0000
40 4160 00 0000 0000
40 4160 00 0000 0000
40 4160 00 0000 0000
22 7a60 00 b03c ffff  -50000 Target Position
40 4160 00 0000 0000
40 4060 00 0000 0000
22 4060 00 7f00 0000  Controlword (relative pos., start immediately)
40 4160 00 0000 0000
40 4160 00 0000 0000
22 4060 00 0f00 0000  Controlword (Switch-on)
22 8160 00 cccc cccc  Profile velocity
40 7a60 00 0000 0000　0 Target Position
40 6460 00 0000 0000 Position actual value

修正を始める

DWORD move(DWORD pTargetPosition, DWORD pProfileVelocity)
{
    BOOL Absolute = 0;
    BOOL Immediately = TRUE;
    VCS_SetDisableState(keyHandle, nodeId, &errorCode);
    VCS_SetEnableState(keyHandle, nodeId, &errorCode);
    VCS_MoveToPosition(keyHandle, nodeId, pTargetPosition, Absolute, Immediately, &errorCode);
    void* Profile_velocity;
    DWORD NbOfBytesWritten = 0;
    VCS_SetObject(keyHandle, nodeId, 0x6081, 0x00, &Profile_velocity, 4, &NbOfBytesWritten, &errorCode);
    printf("\nNbOfBytesWritten:%ld\n",NbOfBytesWritten);
    printf("eroor:%ld", errorCode);
    return errorCode;
}

書き込んだ値とエラー・コードを表示するようにmove()関数を修正して実行しました。

　エラーコードは16進で0x06090030でした。検索すると、value range of parameter exceeded（パラメータの値の範囲を超えました）でした。
VCS_SetObject(keyHandle, nodeId, 0x6081, 0x00, &Profile_velocity, 4, &NbOfBytesWritten, &errorCode);の速度の指定が間違っていますね。
VCS_SetObject(keyHandle, nodeId, 0x6081, 0x00, &pProfileVelocity, 4, &NbOfBytesWritten, &errorCode);に修正して実行しました。

　エラーがなくなりました。
　また、速度の書き込みはSDOです。目標値はPDOで更新されると思われます。そこで、速度の書き込みを、目標値の前に持ってきました。
　２か所直したmove()関数の修正版です。

DWORD move(DWORD pTargetPosition, DWORD pProfileVelocity)
{
    BOOL Absolute = 0;
    BOOL Immediately = TRUE;
    DWORD NbOfBytesWritten = 0;
    VCS_SetDisableState(keyHandle, nodeId, &errorCode);
    VCS_SetObject(keyHandle, nodeId, 0x6081, 0x00, &pProfileVelocity, 4, &NbOfBytesWritten, &errorCode);
    VCS_SetEnableState(keyHandle, nodeId, &errorCode);
    VCS_MoveToPosition(keyHandle, nodeId, pTargetPosition, Absolute, Immediately, &errorCode);
    return errorCode;
}

　このライブラリは必ずエラー・コードが返ります。また、状態をチェックする関数が多く用意されているので、信頼度の高いプログラムを記述できます。

You get articles that match your needs
You can efficiently read back useful information
You can use dark theme

What you can do with signing up