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

　CSPがマップされている元はPosition Mode (PM) です。
   https://maxonjapan.com/wp-content/uploads/manual/epos/EPOS_Command_Library.pdf

 このモードの中に、
- VCS_ActivateAnalogPositionSetpoint
- VCS_DeactivateAnalogPositionSetpoint
- VCS_DisableAnalogPositionSetpoint
があります。ここでは、EPOS4に用意されている2ポートのアナログ入力を利用します。

### アナログ入力を生かす
　EPOS Studio 3.7を立ち上げます。WizardsのStartupを起動します。ID=5だけの設定です。
Analog Inputの1と2の両方をGenral purposeに修正して、finishします。これで、電圧入力になります。

![2022-06-11 (5).png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/407635/b1c89be8-a2cc-2d43-3a38-ac3d84449275.png)

　5V電源（5番ピン）を利用し、ボリュームをつなげます。これで0～+5V入力ができたと思ったのですが、マイナス側を少し検知してしまいます。アナログ入力自体は±を扱います。
　オブジェクト・ディレクトリで電圧を見ると、0V付近から5V付近まで、可変できていることがわかります。1.25Vのような表示ですが、実際に読み出せるのは百倍された整数値です。

<img width="
250" alt="ファイル名" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/407635/f33b1286-a4c6-760b-5d35-7db5c836de9c.png">


　コネクタは、EPOS4のX8コネクタにつなぎます。5VはX7コネクタからもらいました。

<img width="450" alt="ファイル名" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/407635/817be031-3025-64f5-cfcf-ec3207ad3127.png">

　信号ケーブル8芯（520853）、7芯（520854）は純正を購入しました。片一方はコネクタで、他端はバラ状態です。

　ディジタル入力も、使わないならNoneに設定しておきます。

### プログラム

　1万回のループにしています。何らかのアクシデントで止まることがあり、原因が突き止められていません。ボリュームをゆっくり動かしているだけなら、止まりません。
　ボリュームの位置で回転速度が変わります。左右独立しています。
　どうしてもエラーが生じたときに再開できないことがあったので、最初と最後に``VCS_SendNMTService(keyHandle, nodeId, 130, &errorCode); // RESET_COMMUNICATION``を追加しています。
　デフォルトの速度を上げるには、currentValueXを大きくします。


```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 NbOfBytesRead = 0;
int32_t pPositionMust = 0;
void* p1Data = 0;
void* p2Data = 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_SendNMTService(keyHandle, nodeId, 130, &errorCode); // RESET_COMMUNICATION
    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
    long Profile_deceleration = 10000;  // rpm/s
    VCS_SetObject(keyHandle, nodeId, 0x6084, 0x00, &Profile_deceleration, 4, &NbOfBytesWritten, &errorCode);
    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
    //int32_t Max_position_range_limit = 10000;

    VCS_SetObject(keyHandle, nodeId, 0x60b2, 0x00, &Torque_offset, 2, &NbOfBytesWritten, &errorCode);
    VCS_SetObject(keyHandle, nodeId, 0x60b0, 0x00, &Position_offset, 4, &NbOfBytesWritten, &errorCode);
    //VCS_SetObject(keyHandle, nodeId, 0x607b, 0x02, &Max_position_range_limit, 4, &NbOfBytesWritten, &errorCode);

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

    VCS_ActivateAnalogPositionSetpoint(keyHandle, nodeId, 1, 0, 32767, &errorCode);
    VCS_ActivateAnalogPositionSetpoint(keyHandle, nodeId, 2, 0, 32767, &errorCode);
    VCS_EnableAnalogPositionSetpoint(keyHandle, nodeId, &errorCode); 

    return errorCode;
}

uint32_t Reset_state(uint16_t nodeId){
    VCS_ResetDevice(keyHandle, nodeId, &errorCode);
    VCS_SetDisableState(keyHandle, nodeId, &errorCode);
    VCS_SendNMTService(keyHandle, nodeId, 130, &errorCode); // RESET_COMMUNICATION
    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);

        int32_t oldValue1 = 0;
        int32_t oldValue2 = 0;
        int32_t currentValue1 = 0;
        int32_t currentValue2 = 0;
        int32_t addedValue1 = 0;
        int32_t addedValue2 = 0;

        for (int i=1; i<10000; i++){
            VCS_GetObject(keyHandle, nodeId, 0x3160, 0x01, &p1Data, 2, &NbOfBytesRead,&errorCode);
            VCS_GetObject(keyHandle, nodeId, 0x3160, 0x02, &p2Data, 2, &NbOfBytesRead,&errorCode);
            currentValue1 = (int)p1Data/50;
            currentValue2 = (int)p2Data/50;
            printf(":%d: A1 currentValue1 %d oldValue1 %d addedValue1 %d\n",i,currentValue1,oldValue1,addedValue1);
            printf(":%d: A1 currentValue2 %d oldValue2 %d addedValue2 %d\n",i,currentValue2,oldValue2,addedValue2);

            if (currentValue1 - oldValue1 >= 0) {
                moves(5,addedValue1 + currentValue1 );
            }else{
                moves(5,addedValue1 - currentValue1 );
            }

            if (currentValue2 - oldValue2 >= 0) {
                moves(6,-(addedValue2 + currentValue2) );
            }else{
                moves(6,-(addedValue2 - currentValue2) );
            }

            oldValue1 = currentValue1;
            oldValue2 = currentValue2;
            addedValue1 = addedValue1 + currentValue1;
            addedValue2 = addedValue2 + currentValue2;
            printPosition(5);  printPosition(6);
            sleep(0.2);
        }

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

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

　実行が終わった状態です。
![epos301.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/407635/4726ed04-e5be-2bff-806c-535629dd13d1.png)



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