日記；回すだけIII ⑱ ラズパイ、C++でプログラミング＜その4 モータを回すプログラム（EPOS4）＞ベータ版

　　[日記；回すだけIII ⑭ C++でプログラミング＜その3 モータを回すプログラム（EPOS4）＞ベータ版](https://qiita.com/jamjam/items/4b9aff43aa0dd0a9dba4)
のWindowsプログラムをラズパイ環境で動かします。
　回転（位置の移動）部分はmove()関数にしました。VCS_MoveToPosition()は指定した位置まで動かしますが、速度の指定はできないので、Profile_velocity（0x6081）に直接書き込んでいます。
　　台形駆動で目標位置まで動かします。
![68747470733a2f2f71696974612d696d6167652d73746f72652e73332e61702d6e6f727468656173742d312e616d617a6f6e6177732e636f6d2f302f3430373633352f31333431326163662d313536612d313839362d646532612d3863373066313037333130362e706e67.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/407635/fc317cb6-88ae-ca18-25b2-e643143821e1.png)

　ProfileAcceleration（加速） とProfileDeceleration（減速） は最初に書き込んでおいた速度値を使います。move関数では、TargetPositionとProfileVelocityを引数で渡します。TargetPositionはPDOで、ProfileVelocityはSDOの書き込みです。



### プログラム
　ファイル名はepos2_a.cppにしました。MakeFileの中の``TARGET = epos2_a``を変更します。


```cpp
#include <iostream>
#include "Definitions.h"
#include <unistd.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 = 5;

uint16_t State = 0;
uint32_t ProfileVelocity = 1000;
uint32_t ProfileAcceleration = 1000;
uint32_t ProfileDeceleration = 1000;
int32_t PositionIs = 0;
long TargetPosition = 0;

uint32_t move(int32_t pTargetPosition, long pProfileVelocity){
    uint8_t Absolute = 0;
    uint8_t Immediately = 1;
    uint32_t 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;
}

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

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

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

        // 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, 100);

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

        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(3);

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

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

　実行例です。前回のアマゾンで購入したボードで動かしています。CW方向へ10000（速度100）、CCW方向へ50000（速度2000）動かしました。エラー処理には対応していません。

![2022-05-14 (3).png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/407635/7a0b6280-1add-8f80-b50c-fc7e61b98424.png)


### 最初と最後の処理
　前回、スタート時に、

```cpp
VCS_SendNMTService(keyHandle, nodeId, 130, &errorCode); // RESET_COMMUNICATION
VCS_ClearFault(keyHandle, nodeId, &errorCode); // ClearFault
```
で始めました。ここでは、

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

です。プログラムを動かす前に、何らかの異常状態が起こっていても、できるかぎり自分のプログラム内でスマートに処理がしたいのですが、pythonの時から、うまく処理できていません。共通しているのは``VCS_ClearFault()``だけです。
　CANopenのコントローラには、状態を示すLEDが用意されていて、バス・エラーや、内部処理で止まっていることを知らせてくれます。その様子を見ながら、NMTとstateの二つの状態をリセットする形で対処しています。けれど、pythonと、EPOS4のライブラリでは、用意されている関数名が異なり、同じ対応をしているのか、そうでないかがよくわからないので、やみくもに関数を並べているのが現状です。

　終わりの処理は、前回、

```cpp
    VCS_SetState(keyHandle, nodeId, 0x0002, &errorCode);  // Quick stop state
    VCS_SendNMTService(keyHandle, nodeId, 130, &errorCode); // RESET_COMMUNICATION

    VCS_CloseDevice(keyHandle, &errorCode);
```

今回は、

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

どちらも、コントローラのLEDは正常な状態を示しています。ほかのコントローラもアラートは出していません。