日記；回すだけⅣ ⑦ ラズパイ＋EPOS4、C++でプログラミング＜モータの回転 PPM＞

　EPOS4では、次のモードがサポートされています。
- Homing Mode (HMM)　原点復帰
- Profile Position Mode (PPM)　位置決め制御のこと。加速、減速をEPOSが制御する
- Profile Velocity Mode (PVM)　回転数制御のこと。加速、減速をEPOSが制御する
- Cyclic Synchronous Position Mode (CSP)　サイクル同期位置制御
- Cyclic Synchronous Velocity Mode (CSV)　回転数制御モード
- Cyclic Synchronous Torque Mode (CST)　サイクル同期回転数制御

　最初にPPMで回転します。できたら、CSPに挑戦します。どちらも、https://maxonjapan.com/column/column_21/ に解説があります。
![台形a.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/407635/6c75015a-590e-6a19-45a9-394de49d8f63.png)


### PPMの駆動手順
　[EPOS4 Application Notes](https://maxonjapan.com/wp-content/uploads/manual/epos4/EPOS4-Application-Notes-Collection-En.pdf)に上記の動作モードの手順が書かれています。「7.4 Profile Position Mode (PPM)」に従って、EPOS4のライブラリの関数を使って設定します。

(A) オペレーション・モードのセット
　PPMは1です。

|Object name |Object |User value [default value]|
|:-----------|------------:|:------------:|
|Modes of operation |0x6060-00 |0x01 (Profile Position Mode)|

```cpp
VCS_SetOperationMode(keyHandle, nodeId, 1, &errorCode);
```

(B) 初期化のパラメタ

|Object name |Object |User value [default value]|
|:-----------|------------:|:------------:|
|Following error window |0x6065-00 |User-specific [2'000 inc]|
|Max profile velocity |0x607F-00 |Motor-specific [50'000 rpm]|
|Profile velocity |0x6081-00 |Desired velocity [1'000 rpm]|
|Profile acceleration |0x6083-00 |User-specific [10'000 rpm/s]|
|Profile deceleration |0x6084-00 |User-specific [10'000 rpm/s]|
|Quick stop deceleration |0x6085-00 |User-specific [10'000 rpm/s]|
|Motion profile type |0x6086-00 |User-specific [0]|

　最初の四つは、指定する関数がないので、オブジェクトをSDOで書き込みました。これらはコメントアウトしても、動作に影響はなかったです。

```cpp
        long Following_error_window = 2000;  // inc
        VCS_SetObject(keyHandle, nodeId, 0x6065, 0x00, &Following_error_window, 4, &NbOfBytesWritten, &errorCode);
        long Max_profile_velocity = 50000;  // rpm
        VCS_SetObject(keyHandle, nodeId, 0x607f, 0x00, &Max_profile_velocity, 4, &NbOfBytesWritten, &errorCode);
        long Quick_stop_deceleration = 10000;  // rpm/s
        VCS_SetObject(keyHandle, nodeId, 0x6085, 0x00, &Quick_stop_deceleration, 4, &NbOfBytesWritten, &errorCode);
        int8_t Motion_profile_type = 0;
        VCS_SetObject(keyHandle, nodeId, 0x6086, 0x00, &Motion_profile_type, 1, &NbOfBytesWritten, &errorCode);
```

　次の三つは、PPMには必須なので書き込む関数が用意されています。

```cpp
        ProfileVelocity = 2500;
        ProfileAcceleration = 1000;
        ProfileDeceleration = 1000;

        VCS_SetPositionProfile(keyHandle, nodeId, ProfileVelocity, ProfileAcceleration, ProfileDeceleration, &errorCode);
```

(C) デバイスをenableにする

|Object name |Object |User value [default value]|
|:-----------|------------:|:------------:|
|Controlword (Shutdown) |0x6040-00 |0x0006|
|Controlword (Switch on & Enable) |0x6040-00 |0x000F|

　ControlwordのLSBから4ビットの内容です。0x0006はQuick stop=Shutdown、0x000FはSwitched on ＆ Enable operationなのだそうですが、ちょっと不可解です。

|bit3|bit2|bit1|bit0|
|:-----------|------------:|:----------:|:---------:|
|Enable operation|Quick stop|Enable voltage|Switched on|


```cpp
 VCS_SetDisableState(keyHandle, nodeId, &errorCode);

 VCS_SetEnableState(keyHandle, nodeId, &errorCode); 
```

　VCS_SetDisableState()で0x0006が、VCS_SetEnableState()で0x000FがCANバスに出力されたのは、Wiresharkで実際に観測しています。[日記；回すだけIII ⑭ C++でプログラミング＜その3 モータを回すプログラム（EPOS4）＞ベータ版](https://qiita.com/jamjam/items/4b9aff43aa0dd0a9dba4)


(D) Target position

|Object name |Object |User value [default value]|
|:-----------|------------:|:------------:|
|Target position |0x607A-00 |Desired position [inc]|


(E) Relative Positioning 

|Object name |Object |User value [default value]|
|:-----------|------------:|:------------:|
|Controlword (relative position, start immediately) |0x6040-00 |0x007F|

　(D)＋(E)をVCS_MoveToPosition()関数で実行します。PDOマッピングの設定ができていないと、不可解な動作をしました。
```cpp
    uint8_t Absolute = 0;
    uint8_t Immediately = 1;
    VCS_MoveToPosition(keyHandle, nodeId, pTargetPosition, Absolute, Immediately, &errorCode); 
```

(F) Controlword (New Position)

|Object name |Object |User value [default value]|
|:-----------|------------:|:------------:|
|Controlword (New Position) |0x6040-00 |0x000F (toggle “New Position”)|

　実行していません。

### EPOS Studio 3.7でオブジェクト・ディレクトリを見る



　ID=5のオブジェクト・ディレクトリの実測値です。
![2022-06-07 (3).png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/407635/d71fbcd2-2cc3-e209-f9ae-8d8de2eaf56e.png)

　ID=6のオブジェクト・ディレクトリの実測値です。

![2022-06-07 (2).png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/407635/18538422-4420-1e1b-01fb-ccf74a9521eb.png)

### プログラム
　ID=5とID=6を同時に駆動するのに、同じ命令を並べて記述しました。回転と速度の指定はmove()関数にしました。
　move(10000, 100);はCWでゆっくり、move(-50000, 2000);はCCWに高速で長めに回転します。
　二つのモータはずれずに動いているように見えます。ただ、一つはチューニングがうまくできず、騒音を出して動かしていた時期が長く、チューニング後の二つのPI値は少し異なります。止まるときの動きは少し微妙に異なるように見えます。
　まっすぐに走らせたいとき、ID=5とID=6を独立したmove関数にし、CANopenのSYNC（たぶん100ms同期）によって、数ms以内に同時に実行されるかもっとずれるかなどはまだ実験していません。

```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;
uint16_t nodeId = 5;

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

uint32_t move(int32_t pTargetPosition, long pProfileVelocity){
    uint8_t Absolute = 0;
    uint8_t Immediately = 1;
    
    VCS_SetDisableState(keyHandle, nodeId, &errorCode);  // Controlword (Shutdown) 0x0006
    VCS_SetDisableState(keyHandle, nodeId+1, &errorCode);

    VCS_SetObject(keyHandle, nodeId, 0x6081, 0x00, &pProfileVelocity, 4, &NbOfBytesWritten, &errorCode);
    VCS_SetObject(keyHandle, nodeId+1, 0x6081, 0x00, &pProfileVelocity, 4, &NbOfBytesWritten, &errorCode);

    VCS_SetEnableState(keyHandle, nodeId, &errorCode);  // Controlword (Switch on & Enable) 0x000F
    VCS_SetEnableState(keyHandle, nodeId+1, &errorCode);

    VCS_MoveToPosition(keyHandle, nodeId, pTargetPosition, Absolute, Immediately, &errorCode);  // Target position
                                                      // Controlword (relative position, start immediately) 0x007F
    VCS_MoveToPosition(keyHandle, nodeId+1, -1*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_ClearFault(keyHandle, nodeId+1, &errorCode);
        VCS_SetEnableState(keyHandle, nodeId, &errorCode);
        VCS_SetEnableState(keyHandle, nodeId+1, &errorCode);

        // Initialisation
        VCS_SetDisableState(keyHandle, nodeId, &errorCode);
        VCS_SetDisableState(keyHandle, nodeId+1, &errorCode);
        VCS_SetOperationMode(keyHandle, nodeId, 1, &errorCode);  // 0x01 (Profile Position Mode)
        VCS_SetOperationMode(keyHandle, nodeId+1, 1, &errorCode);

        long Following_error_window = 2000;  // inc
        VCS_SetObject(keyHandle, nodeId, 0x6065, 0x00, &Following_error_window, 4, &NbOfBytesWritten, &errorCode);
        VCS_SetObject(keyHandle, nodeId+1, 0x6065, 0x00, &Following_error_window, 4, &NbOfBytesWritten, &errorCode);
        long Max_profile_velocity = 50000;  // rpm
        VCS_SetObject(keyHandle, nodeId, 0x607f, 0x00, &Max_profile_velocity, 4, &NbOfBytesWritten, &errorCode);
        VCS_SetObject(keyHandle, nodeId+1, 0x607f, 0x00, &Max_profile_velocity, 4, &NbOfBytesWritten, &errorCode);
        long Quick_stop_deceleration = 10000;  // rpm/s
        VCS_SetObject(keyHandle, nodeId, 0x6085, 0x00, &Quick_stop_deceleration, 4, &NbOfBytesWritten, &errorCode);
        VCS_SetObject(keyHandle, nodeId+1, 0x6085, 0x00, &Quick_stop_deceleration, 4, &NbOfBytesWritten, &errorCode);
        int8_t Motion_profile_type = 0;
        VCS_SetObject(keyHandle, nodeId, 0x6086, 0x00, &Motion_profile_type, 1, &NbOfBytesWritten, &errorCode);
        VCS_SetObject(keyHandle, nodeId+1, 0x6086, 0x00, &Motion_profile_type, 1, &NbOfBytesWritten, &errorCode);

        ProfileVelocity = 2500;
        ProfileAcceleration = 1000;
        ProfileDeceleration = 1000;

        VCS_SetPositionProfile(keyHandle, nodeId, ProfileVelocity, ProfileAcceleration, ProfileDeceleration, &errorCode);
        VCS_SetPositionProfile(keyHandle, nodeId+1, ProfileVelocity, ProfileAcceleration, ProfileDeceleration, &errorCode);

        VCS_GetPositionIs(keyHandle, nodeId, &PositionIs, &errorCode);
        printf("\nID5 org positionIs--- %ld\n", PositionIs);
        VCS_GetPositionIs(keyHandle, nodeId+1, &PositionIs, &errorCode);
        printf("\nID6 org positionIs--- %ld\n", PositionIs);

        // move

        move(10000, 100);

        VCS_GetTargetPosition(keyHandle, nodeId, &TargetPosition, &errorCode);
        printf("\nID5 TargetPosition--- %ld\n", TargetPosition);
        VCS_GetTargetPosition(keyHandle, nodeId+1, &TargetPosition, &errorCode);
        printf("\nID6 TargetPosition--- %ld\n", TargetPosition);
        sleep(3);

        VCS_GetPositionIs(keyHandle, nodeId, &PositionIs, &errorCode);
        printf("\nID5 new positionIs--- %ld\n", PositionIs);
        VCS_GetPositionIs(keyHandle, nodeId+1, &PositionIs, &errorCode);
        printf("\nID6 new positionIs--- %ld\n", PositionIs);

        move(-50000, 2000);

        VCS_GetTargetPosition(keyHandle, nodeId, &TargetPosition, &errorCode);
        printf("\nID5 TargetPosition--- %ld\n", TargetPosition);
        VCS_GetTargetPosition(keyHandle, nodeId+1, &TargetPosition, &errorCode);
        printf("\nID6 TargetPosition--- %ld\n", TargetPosition);
        sleep(3);

        VCS_GetPositionIs(keyHandle, nodeId, &PositionIs, &errorCode);
        printf("\nID5 new positionIs--- %ld\n", PositionIs);
        VCS_GetPositionIs(keyHandle, nodeId+1, &PositionIs, &errorCode);
        printf("\nID6 new positionIs--- %ld\n", PositionIs);

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


##### 目次
- [① ラズパイ＋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)