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

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

### NMTとStatus Machineはどこで使われているのか
　[EPOS Command Library Documentation](https://maxonjapan.com/wp-content/uploads/manual/epos/EPOS_Command_Library.pdf)の中に、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本とも抜いておきます。

```cpp
cd works
ls
```

![eps01.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/407635/1474b52a-21a5-10ac-2847-e3d735f49cfb.png)

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


![eps02a.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/407635/cb5c1e74-73c2-e17d-3b46-b3dcf4271f97.png)

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

![2022-06-05 (13).png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/407635/629b08e0-e263-87e3-8db9-66c2644af24a.png)

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

![2022-06-05 (15).png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/407635/2f4817ab-f718-25ea-e94c-e34f059740df.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()が充実しているので、今の状態を確認しながら何かを実行するというプログラミング・スタイルがベターだと言えます。

```cpp
#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*``に変換する形に変更したプログラムです。

```cpp
#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");
}
```

##### 目次
- [① ラズパイ＋EPOS4、C++でプログラミング＜準備＞](https://qiita.com/jamjam/items/f6acf1128c6020b27059)
- [② ラズパイ＋EPOS4、C++でプログラミング＜車体の用意＞](https://qiita.com/jamjam/items/a9d400baaa5909bb0885)
- [③ ラズパイ＋EPOS4、C++でプログラミング＜モータのチューニング＞](https://qiita.com/jamjam/items/9266eb684d8a2bec7ac6)
- [④ ラズパイ＋EPOS4、C++でプログラミング＜モータのチューニング2＞](https://qiita.com/jamjam/items/93a57e9356bb1f9d1e6a)
- [⑤ ラズパイ＋EPOS4、C++でプログラミング＜ライブラリの準備＞](https://qiita.com/jamjam/items/be97d25e90c7aa9b92c5)
- [⑥ ラズパイ＋EPOS4、C++でプログラミング＜オブジェクト・ディクショナリを読み出す＞](https://qiita.com/jamjam/items/0cd6c5363c851c3c0e3c)
- [⑦ ラズパイ＋EPOS4、C++でプログラミング＜モータの回転 PPM＞](https://qiita.com/jamjam/items/725047121f582bb30937)
- [⑧ ラズパイ＋EPOS4、C++でプログラミング＜モータの回転 CSP その1＞ベータ版](https://qiita.com/jamjam/items/d58bf9affbd980e1735b)
- [⑨ ラズパイ＋EPOS4、C++でプログラミング＜モータの回転 CSP その2＞ベータ版](https://qiita.com/jamjam/items/ea611e552c5787ed5de0)