LoginSignup
1
0

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?

More than 1 year has passed since last update.

@jamjam(吉田 伸三)

日記；回すだけⅣ ⑥ ラズパイ＋EPOS4、C++でプログラミング＜オブジェクト・ディクショナリを読み出す＞

Last updated at Posted at 2022-06-06

　SDOでオブジェクト・ディクショナリの項目を読み出すプログラムを作ります。

NMTとStatus Machineはどこで使われているのか

　EPOS Command Library Documentationの中に、NMT関連の関数を探します。一つ見つかりました。

  • VCS_SendNMTService
状態 パラメタ 内容
　Start remote node 　　 1 　　 NCS_START_REMOTE_NODE
　Stop remote node 　 　 2 　　 NCS_STOP_REMOTE_NODE
　Enter pre-operational 128 　 NCS_ENTER_PRE_OPERATIONAL
　Reset node 　　　　　　 129 　 NCS_RESET_NODE
　Reset communication 　 130 　 NCS_RESET_COMMUNICATION

　サンプル・プログラムHelloEposCmdで検索しましたが、使われていないようです。

　Status Machine関連の関数を探します。

  • VCS_ResetDevice
  • VCS_SetState
状態 パラメタ 内容
Get/Set Disable State 0x0000 ST_DISABLED
Get/Set Enable State 0x0001 ST_ENABLED
Get/Set Quickstop State 0x0002 ST_QUICKSTOP
Get Fault State 0x0003 ST_FAULT
  • VCS_SetEnableState
  • VCS_SetDisableState
  • VCS_SetQuickStopState
  • VCS_ClearFault
  • VCS_GetState
  • VCS_GetEnableState
  • VCS_GetDisableState
  • VCS_GetQuickStopState
  • VCS_GetFaultState

　サンプル・プログラムHelloEposCmdで検索すると、次の関数が使われていることがわかりました。

VCS_SetEnableState
VCS_SetDisableState
VCS_ClearFault
VCS_GetEnableState
VCS_GetFaultState

プログラムの実行

　worksディレクトリで作業をしています。USBケーブルは2本とも抜いておきます。

cd works
ls

eps01.png

　includeするのはDefinitions.hです。VCS_xxx関数が初期化されています。プログラム名をepos.cppにしました。makeするためのMakefileをテキスト・エディタで編集します。TRAGETのところをTARGET = eposに変更します。

eps02a.png

　makeし、./eposで実行します。

2022-06-05 (13).png

　epos.cppのnodeIdを6に変更し、make、./eposで実行します。

2022-06-05 (15).png

　最後の2行のPI制御の値が微妙に異なっています。モータの特性の違いかもしれません。

プログラム①

　VCS_OpenDevice()を起動してエラーがなかった時点で、NMTはInitialization状態からPre-operational状態をへて、Operational状態に移行していると思われます。しかし、ライブラリの中に、NMT状態を表示する関数が見つかっていません。
　SDOで読み出すとき、Pre-operational状態でもできたと思いますが、確認はできていません。
　コメントアウトしていますが、VCS_SendNMTService()のRESET_COMMUNICATIONは、CANバスでほかのデバイスが起こしたエラーなどをリセットします。このEPOS4が2台の構成では、そういう事態は起こらないかもしれません。
　Status Machineの状態も、Operation enabled状態になっているものと想像します。VCS_ClearFault()を念のために実行しています。このままだとOperation enabled状態に遷移していないと思いますが、SDOの読み出しはできています。
　読み出しが終わったら、VCS_SetQuickStopState()でSwitch on disabled状態に遷移させたうえで、最終処理のVCS_CloseDevice()を実行させています。VCS_CloseDevice()だけだと、EPOS4のLEDのエラー表示が止まらなかったことがあるためです。
　Status Machineの状態は、getXXXState()が充実しているので、今の状態を確認しながら何かを実行するというプログラミング・スタイルがベターだと言えます。

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

void* keyHandle = 0;
char* deviceName = (char*)"EPOS4";
char* protocolStackName = (char*)"CANopen";
char* interfaceName = (char*)"CAN_mcp251x 0";
char* portName = (char*)"CAN0";
uint32_t errorCode = 0;
uint32_t timeout = 0;
uint16_t nodeId = 6;
uint16_t MotorType = 0;
uint32_t NominalCurrent = 0;
uint32_t MaxOutputCurrent = 0;
uint16_t ThermalTimeConstant = 0;
uint8_t  NbOfPolePairs = 0;
uint16_t SensorType = 0;
uint32_t EncoderResolution = 0;
int32_t  InvertedPolarity = 0;
uint16_t EController = 1; // PI current controller
uint16_t EGain = 1;  // 1:Current controller P gain, 2:Current controller I gain
uint64_t Value = 0;

int main(){
    printf("start EPOS4\n");
    printf(" interfaceName is %s , protocol is %s\n", interfaceName,protocolStackName);

    keyHandle = VCS_OpenDevice(deviceName, protocolStackName, interfaceName, portName, &errorCode);
    printf(" keyHandle  is 0x%x , errorCode  is 0x%x\n", keyHandle,errorCode);

    //VCS_SendNMTService(keyHandle, nodeId, 130, &errorCode); // RESET_COMMUNICATION
    VCS_ClearFault(keyHandle, nodeId, &errorCode); // ClearFault

    printf("\n - - - - - \n");

    if (keyHandle!=0 && errorCode == 0) {

        VCS_Store(keyHandle, nodeId, &errorCode);
        printf("nodeId  is %d\n", nodeId);

        VCS_GetMotorType(keyHandle, nodeId, &MotorType, &errorCode);
        printf("MotorType:(10 is EC motor sinus commutated): %d\n", MotorType);

        VCS_GetDcMotorParameterEx(keyHandle, nodeId, &NominalCurrent, &MaxOutputCurrent, &ThermalTimeConstant, &errorCode);
        printf("NominalCurrent: %d  MaxOutputCurrent: %d ThermalTimeConstant: %d\n", NominalCurrent, MaxOutputCurrent, ThermalTimeConstant);

        VCS_GetEcMotorParameterEx(keyHandle, nodeId, &NominalCurrent, &MaxOutputCurrent, &ThermalTimeConstant, &NbOfPolePairs, &errorCode);
        printf("EcMotor NbOfPolePairs: %d\n", NbOfPolePairs);

        VCS_GetSensorType(keyHandle, nodeId, &SensorType, &errorCode);
        printf("SensorType:(1 is Incremental encoder 1 with index (3-channel)): %d\n", SensorType);

        VCS_GetIncEncoderParameter(keyHandle, nodeId, &EncoderResolution, &InvertedPolarity, &errorCode);
        printf("EncoderResolution: %d InvertedPolarity: %d\n", EncoderResolution, InvertedPolarity);

        VCS_GetControllerGain(keyHandle, nodeId, EController, 1, &Value, &errorCode);
        printf("CurrentController PGain: %.3fmV/A\n", Value / 1000.0);
        VCS_GetControllerGain(keyHandle, nodeId, EController, 2, &Value, &errorCode);
        printf("CurrentController IGain: %.3fmV/(A*ms)\n", Value / 1000.0);
    }

    VCS_SetQuickStopState(keyHandle, nodeId, &errorCode);  // Quick stop state
    //VCS_SendNMTService(keyHandle, nodeId, 130, &errorCode); // RESET_COMMUNICATION

    VCS_CloseDevice(keyHandle, &errorCode);
    printf("\nClose device\n");
}

プログラム②

　VCS_OpenDevice()の引数がchar*になっています。サンプル・プログラムHelloEposCmdのように、もともとの文字列をstring型にして、char*に変換する形に変更したプログラムです。

#include <stdlib.h>
#include <iostream>
#include "Definitions.h"
#include <string>
#include <cstring>
using namespace std;

void* keyHandle = 0;
string g_deviceName = "EPOS4";
string g_protocolStackName = "CANopen";
string g_interfaceName = "CAN_mcp251x 0";
string g_portName = "CAN0";
uint32_t errorCode = 0;
uint32_t timeout = 0;
uint16_t nodeId = 6;
uint16_t MotorType = 0;
uint32_t NominalCurrent = 0;
uint32_t MaxOutputCurrent = 0;
uint16_t ThermalTimeConstant = 0;
uint8_t  NbOfPolePairs = 0;
uint16_t SensorType = 0;
uint32_t EncoderResolution = 0;
int32_t  InvertedPolarity = 0;
uint16_t EController = 1; // PI current controller
uint16_t EGain = 1;  // 1:Current controller P gain, 2:Current controller I gain
uint64_t Value = 0;

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

    char* pDeviceName = new char[255];  // メモリ確保
    char* pProtocolStackName = new char[255];
    char* pInterfaceName = new char[255];
    char* pPortName = new char[255];

    strcpy(pDeviceName, g_deviceName.c_str());  // コピー
    strcpy(pProtocolStackName, g_protocolStackName.c_str());
    strcpy(pInterfaceName, g_interfaceName.c_str());
    strcpy(pPortName, g_portName.c_str());

    printf(" interfaceName is %s , protocol is %s\n", pInterfaceName, pProtocolStackName);

    keyHandle = VCS_OpenDevice(pDeviceName, pProtocolStackName, pInterfaceName, pPortName, &errorCode);
    printf(" keyHandle  is 0x%x , errorCode  is 0x%x\n", keyHandle,errorCode);

    //VCS_SendNMTService(keyHandle, nodeId, 130, &errorCode); // RESET_COMMUNICATION
    VCS_ClearFault(keyHandle, nodeId, &errorCode); // ClearFault

    printf("\n - - - - - \n");

    if (keyHandle!=0 && errorCode == 0) {

        VCS_Store(keyHandle, nodeId, &errorCode);
        printf("nodeId  is %d\n", nodeId);

        VCS_GetMotorType(keyHandle, nodeId, &MotorType, &errorCode);
        printf("MotorType:(10 is EC motor sinus commutated): %d\n", MotorType);

        VCS_GetDcMotorParameterEx(keyHandle, nodeId, &NominalCurrent, &MaxOutputCurrent, &ThermalTimeConstant, &errorCode);
        printf("NominalCurrent: %d  MaxOutputCurrent: %d ThermalTimeConstant: %d\n", NominalCurrent, MaxOutputCurrent, ThermalTimeConstant);

        VCS_GetEcMotorParameterEx(keyHandle, nodeId, &NominalCurrent, &MaxOutputCurrent, &ThermalTimeConstant, &NbOfPolePairs, &errorCode);
        printf("EcMotor NbOfPolePairs: %d\n", NbOfPolePairs);

        VCS_GetSensorType(keyHandle, nodeId, &SensorType, &errorCode);
        printf("SensorType:(1 is Incremental encoder 1 with index (3-channel)): %d\n", SensorType);

        VCS_GetIncEncoderParameter(keyHandle, nodeId, &EncoderResolution, &InvertedPolarity, &errorCode);
        printf("EncoderResolution: %d InvertedPolarity: %d\n", EncoderResolution, InvertedPolarity);

        VCS_GetControllerGain(keyHandle, nodeId, EController, 1, &Value, &errorCode);
        printf("CurrentController PGain: %.3fmV/A\n", Value / 1000.0);
        VCS_GetControllerGain(keyHandle, nodeId, EController, 2, &Value, &errorCode);
        printf("CurrentController IGain: %.3fmV/(A*ms)\n", Value / 1000.0);
    }

    delete []pDeviceName;  // メモリ解放
    delete []pProtocolStackName;
    delete []pInterfaceName;
    delete []pPortName;

    VCS_SetQuickStopState(keyHandle, nodeId, &errorCode);  // Quick stop state
    //VCS_SendNMTService(keyHandle, nodeId, 130, &errorCode); // RESET_COMMUNICATION

    VCS_CloseDevice(keyHandle, &errorCode);
    printf("\nClose device\n");
}
目次
1
0
0

Register as a new user and use Qiita more conveniently

  1. You get articles that match your needs
  2. You can efficiently read back useful information
  3. You can use dark theme
What you can do with signing up
Sign upLogin
1
0

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?