日記；回すだけⅣ ⑧ ラズパイ＋EPOS4、C++でプログラミング＜モータの回転 CSP その1＞ベータ版

　位置決めのPPMは、FAなどの制御では必須の動作モードです。サイクル同期位置制御のCSPはロボットなどの動作に向いているようです。まえの世代のコントローラEPOS2では用意されていませんでした。
### CSPとは
　[EPOS4の位置制御モード Profile Position Mode（PPM）とサイクル同期位置制御（CSP）の違いは?](https://maxonjapan.com/column/column_21/) の資料では、指令位置に対して、最短で到達するように制御する、と書かれています。コントローラ内では、位置制御とトルク制御が行われます。
　常に新しいtarget positionをエンドユーザが与え続けます。多軸を同時に制御するようなときに、EtherCATであれば2ms単位で位置指定を更新できます。現在使っているEPOS4はそのタイミングに十分対応しています。けれど、ここで利用しているCANopenでは、時間同期のSYNCは100msと思われます。

### CSPはPM

　[EPOS Command Library Documentation](https://maxonjapan.com/wp-content/uploads/manual/epos/EPOS_Command_Library.pdf)で、Operation Modeを見ます。VCS_SetOperationMode()でCyclic Synchronous Position Mode (CSP) は8なのですが、Position Mode (PM) からマップされていると説明されています。
　ライブラリにはCSPはなく、PMを使うようです。

### CSPの駆動手順
　[EPOS4 Application Notes](https://maxonjapan.com/wp-content/uploads/manual/epos4/EPOS4-Application-Notes-Collection-En.pdf)にCSPの駆動手順が書かれています。EPOS4のライブラリの関数を使って設定します。

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

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

```cpp
VCS_SetOperationMode(keyHandle, nodeId, 0x08, &errorCode); 
```

(B) 初期化のパラメタ

|Object name |Object |User value [default value]|
|:-----------|------------:|:------------:|
|Max motor speed |0x6080-00 |User-specific; motor or mechanical limits [rpm]|
|Max gear input speed |0x3003-03 |User-specific: Gear or mechanical limits [rpm]|
|Profile deceleration [a] |0x6084-00 |User-specific [10'000 rpm/s]|
|Quick-stop deceleration [a] |0x6085-00 |User-specific [10'000 rpm/s]|
|Interpolation time period [b] |0x60C2-xx |Master’s SYNC period (Cycle ticks) [1 ms]|


　指定する関数がないので、オブジェクトをSDOで書き込みました。
　ギアはないので、Max gear input speedは設定していません。Profile decelerationは減速時のパラメタですが、設定し忘れてます。
　Interpolation_time_periodは補完時間で、0から100msが指定できます。EtherCATなら2ms程度だと書かれているので、今使っているCANopenは100msだと思われるので、100を設定してあります。動作が正しく行われているか、まだ確認できていません。

```cpp
    long Max_motor_speed = 10000;  // inc
    VCS_SetObject(keyHandle, nodeId, 0x6080, 0x00, &Max_motor_speed, 4, &NbOfBytesWritten, &errorCode);
    // skip Max gear input speed
    // skip Profile deceleration
    long Quick_stop_deceleration = 10000;  // rpm/s
    VCS_SetObject(keyHandle, nodeId, 0x6085, 0x00, &Quick_stop_deceleration, 4, &NbOfBytesWritten, &errorCode);
    uint8_t Interpolation_time_period = 100;  // 100ms
    VCS_SetObject(keyHandle, nodeId, 0x60c2, 0x01, &Interpolation_time_period, 1, &NbOfBytesWritten, &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) Set offset

|Object name |Object |User value [default value]|
|:-----------|------------:|:------------:|
|Torque offset |0x60B2-00 |Torque offset [per thousand of “Motor rated torque”; 0x6076]|
|Position offset |0x60B0-00 |Position offset [0 inc]|

　Torque_offsetは、オブジェクト・ディレクトリの値を読んで代入しています。

```cpp
    int16_t Torque_offset  = 74;  // read 
    int32_t Position_offset  = 0; // inc
    VCS_SetObject(keyHandle, nodeId, 0x60b2, 0x00, &Torque_offset, 2, &NbOfBytesWritten, &errorCode);
    VCS_SetObject(keyHandle, nodeId, 0x60b0, 0x00, &Position_offset, 4, &NbOfBytesWritten, &errorCode);
```
(E) Set position

　ライブラリのVCS_SetPositionMust()を使ってTarget positionを書き込みます。

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


```cpp
    VCS_SetPositionMust(keyHandle, nodeId, pPositionMust, &errorCode); 
```

(F) Stop positioning

|Object name |Object |User value [default value]|
|:-----------|------------:|:------------:|
|Controlword (Quick stop) |0x6040-00 |0x000B|

　実行していません。

### プログラム
　Position Mode (PM)なら、VCS_ActivatePositionMode()が用意されていますが、何を具体的にするのか不明、調べていないので使いません。
　Set positionはVCS_SetPositionMust()を使いました。
　前回のプログラムではID=5とID=6を同時に記述しましたが、ここでは、ほとんどの処理を関数にし、IDを引数にして見やすくしました。
　positionは、絶対座標？が使われるようなので、最初に、現在位置を0にするためにHomingの37を実行します。実際に動かすわけではないので、スピードなどは設定していません。

```cpp
uint32_t homing(uint16_t nodeId){
    VCS_ActivateHomingMode(keyHandle, nodeId, &errorCode);
    VCS_FindHome(keyHandle, nodeId, 37, &errorCode);  // Actual position
    VCS_StopHoming(keyHandle, nodeId, &errorCode);
    return errorCode;
}
```


　プログラム全体です。

```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;
int32_t pPositionMust = 0;

uint32_t moves(uint16_t nodeId, int32_t pPositionMust){
    VCS_SetPositionMust(keyHandle, nodeId, pPositionMust, &errorCode);  
    return errorCode;
}

uint32_t homing(uint16_t nodeId){
    VCS_ActivateHomingMode(keyHandle, nodeId, &errorCode);
    VCS_FindHome(keyHandle, nodeId, 37, &errorCode);  // Actual position
    VCS_StopHoming(keyHandle, nodeId, &errorCode);
    return errorCode;
}

uint32_t printPosition(uint16_t nodeId){
    VCS_GetPositionIs(keyHandle, nodeId, &PositionIs, &errorCode);
    printf("\nID=%d positionIs--- %ld\n", nodeId,PositionIs);
    return errorCode;
}

uint32_t Initialisation(uint16_t nodeId){
    // enable_state
    VCS_ClearFault(keyHandle, nodeId, &errorCode); 
    VCS_SetEnableState(keyHandle, nodeId, &errorCode);

    // Initialisation
    VCS_SetDisableState(keyHandle, nodeId, &errorCode);
    VCS_SetOperationMode(keyHandle, nodeId, 0x08, &errorCode);  // 0x08 (Cyclic Synchronous Position Mode)

    long Max_motor_speed = 10000;  // inc
    VCS_SetObject(keyHandle, nodeId, 0x6080, 0x00, &Max_motor_speed, 4, &NbOfBytesWritten, &errorCode);
    // skip Max gear input speed
    // skip Profile deceleration
    long Quick_stop_deceleration = 10000;  // rpm/s
    VCS_SetObject(keyHandle, nodeId, 0x6085, 0x00, &Quick_stop_deceleration, 4, &NbOfBytesWritten, &errorCode);
    uint8_t Interpolation_time_period = 100;  // 100ms
    VCS_SetObject(keyHandle, nodeId, 0x60c2, 0x01, &Interpolation_time_period, 1, &NbOfBytesWritten, &errorCode);
    // Nominal torque ;207 mNm µNm
    // Motor Rated Torque is mota tekaku toruku

    int16_t Torque_offset  = 74;  // read dictionary
    int32_t Position_offset  = 0; // inc
    VCS_SetObject(keyHandle, nodeId, 0x60b2, 0x00, &Torque_offset, 2, &NbOfBytesWritten, &errorCode);
    VCS_SetObject(keyHandle, nodeId, 0x60b0, 0x00, &Position_offset, 4, &NbOfBytesWritten, &errorCode);

    VCS_SetDisableState(keyHandle, nodeId, &errorCode);  // Controlword (Shutdown) 0x0006
    VCS_SetEnableState(keyHandle, nodeId, &errorCode);   // Controlword (Switch on & Enable) 0x000F

    return errorCode;
}

uint32_t Reset_state(uint16_t nodeId){
    VCS_ResetDevice(keyHandle, nodeId, &errorCode);
    VCS_SetDisableState(keyHandle, nodeId, &errorCode);
    return errorCode;
}

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

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

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

        homing(5);
        homing(6);

        Initialisation(5);
        Initialisation(6);

        printPosition(5);
        printPosition(6);

        //move

        moves(5,0);
        moves(6,0);
        sleep(0.5);
        printPosition(5);
        printPosition(6);

        moves(5,100);
        moves(6,-100);
        sleep(1);
        printPosition(5);
        printPosition(6);

        moves(5,200);
        moves(6,-200);
        sleep(1);
        printPosition(5);
        printPosition(6);

        moves(5,300);
        moves(6,-300);
        sleep(1);
        printPosition(5);
        printPosition(6);

        moves(5,400);
        moves(6,-400);
        sleep(1);
        printPosition(5);
        printPosition(6);

        moves(5,500);
        moves(6,-500);
        sleep(1);
        printPosition(5);
        printPosition(6);

        moves(5,600);
        moves(6,-600);
        sleep(1);
        printPosition(5);
        printPosition(6);

        moves(5,700);
        moves(6,-700);
        sleep(1);
        printPosition(5);
        printPosition(6);

        moves(5,800);
        moves(6,-800);
        sleep(1);
        printPosition(5);
        printPosition(6);

        printf("\nReset state");
        Reset_state(5);
        Reset_state(6);

    }
    VCS_CloseDevice(keyHandle, &errorCode);
}
```

　EPOS Studio 3.7の中にData Recorderがあります。上記のプログラムをロギングしました。
![2022-06-10 (2).png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/407635/fee606a4-c6d3-bcaf-7e66-039336db8d02.png)

　[EPOS4のサイクル同期位置制御（CSP）の補間機能とは?](https://maxonjapan.com/column/column_22/)の中に補間の話があります。上記のプログラムは1秒ごとにpositionを100incずつ増加させています。1秒以下にすると、動作が瞬間に終わってしまいます。1秒ごとでは補間は行われていません。
　補間はPDO通信のときだけ有効と、[EPOS4 Firmware Specification](https://maxonjapan.com/wp-content/uploads/manual/epos4/EPOS4-Firmware-Specification-En.pdf)の「3.6 Cyclic Synchronous Position Mode (CSP)」に書かれています。
　「3.6.1 How to use «CSP»　COMMANDING PARAMETERS」の中にPDOに関して、Target Position（0x607a）とPosition offset（0x60b0）が関係するような記述があります。Target PositionはRxPDO3にマッピングされていますが、Position offsetはマッピングされていません。
　VCS_SetPositionMust()関数でTarget Positionは使われているようですが、Position offsetはわかりません。
　動作が不明な部分が多々あります。

### PIDのパラメタの不一致
　ID＝6のホイールの回り方が緩いです。左はID＝5のオブジェクト・ディレクトリです。この値を右のID＝6の同じPIDの項目にコピーし、保存します。
![epos201a.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/407635/1280d2b3-f3fd-85e8-fbbb-a2b51df9981e.png)


### ライブラリのVCS_SetPositionMust()のコード
　今まで構成図には入れていましたが、何も使っていなかったIXXAT USB-to_CAN V2 Compactの付属ソフトcanAnalyser3 Miniを使ってCANバスのデータをロギングします。

![2022-06-10 (9).png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/407635/641a83d8-dbf0-2f32-8a38-f314d33742bb.png)
　CSVで保存し、Data(hex)だけ残して、ほかの項目を削除します。メモ帳で開き、「7A 60」で検索します。moves()が連続している付近を見ます。
　右側に手動でコードした内容を併記しています。
　これらから、VCS_SetPositionMust()は0x607AのTarget positionが実行されていることが確認できました。

```
43 64 60 00 00 00 00 00
22 7A 60 00 64 00 00 00 Target position 6400  (25600)
60 7A 60 00 00 00 00 00
22 7A 60 00 9C FF FF FF Target position 9c00 (39936)
60 7A 60 00 00 00 00 00
40 64 60 00 00 00 00 00
43 64 60 00 22 00 00 00
40 64 60 00 00 00 00 00
43 64 60 00 EA FF FF FF
22 7A 60 00 C8 00 00 00 Target position c800  (51200)
60 7A 60 00 00 00 00 00
22 7A 60 00 38 FF FF FF Target position 3800  (14336)
60 7A 60 00 00 00 00 00
40 64 60 00 00 00 00 00
43 64 60 00 98 00 00 00
40 64 60 00 00 00 00 00
43 64 60 00 8F FF FF FF
22 7A 60 00 2C 01 00 00 Target position 2c00  (11264)
60 7A 60 00 00 00 00 00
22 7A 60 00 D4 FE FF FF Target position d400  (54272)
60 7A 60 00 00 00 00 00
40 64 60 00 00 00 00 0
```


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