LoginSignup
1
0

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?

More than 1 year has passed since last update.

@jamjam(吉田 伸三)

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

Last updated at Posted at 2022-06-10

　位置決めのPPMは、FAなどの制御では必須の動作モードです。サイクル同期位置制御のCSPはロボットなどの動作に向いているようです。まえの世代のコントローラEPOS2では用意されていませんでした。

CSPとは

　EPOS4の位置制御モード Profile Position Mode（PPM）とサイクル同期位置制御（CSP）の違いは? の資料では、指令位置に対して、最短で到達するように制御する、と書かれています。コントローラ内では、位置制御とトルク制御が行われます。
　常に新しいtarget positionをエンドユーザが与え続けます。多軸を同時に制御するようなときに、EtherCATであれば2ms単位で位置指定を更新できます。現在使っているEPOS4はそのタイミングに十分対応しています。けれど、ここで利用しているCANopenでは、時間同期のSYNCは100msと思われます。

CSPはPM

　EPOS Command Library Documentationで、Operation Modeを見ます。VCS_SetOperationMode()でCyclic Synchronous Position Mode (CSP) は8なのですが、Position Mode (PM) からマップされていると説明されています。
　ライブラリにはCSPはなく、PMを使うようです。

CSPの駆動手順

　EPOS4 Application NotesにCSPの駆動手順が書かれています。EPOS4のライブラリの関数を使って設定します。

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

Object name Object User value [default value]
Modes of operation 0x6060-00 0x08 (Cyclic Synchronous Position Mode) 
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を設定してあります。動作が正しく行われているか、まだ確認できていません。

    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 
 VCS_SetDisableState(keyHandle, nodeId, &errorCode);

 VCS_SetEnableState(keyHandle, nodeId, &errorCode);

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

(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は、オブジェクト・ディレクトリの値を読んで代入しています。

    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] 
    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を実行します。実際に動かすわけではないので、スピードなどは設定していません。

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;
}

　プログラム全体です。

#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

　EPOS4のサイクル同期位置制御（CSP）の補間機能とは?の中に補間の話があります。上記のプログラムは1秒ごとにpositionを100incずつ増加させています。1秒以下にすると、動作が瞬間に終わってしまいます。1秒ごとでは補間は行われていません。
　補間はPDO通信のときだけ有効と、EPOS4 Firmware Specificationの「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

ライブラリのVCS_SetPositionMust()のコード

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

2022-06-10 (9).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
目次
1
0
0

Register as a new user and use Qiita more conveniently

  1. You get articles that match your needs
  2. You can efficiently read back useful information
  3. You can use dark theme
What you can do with signing up
Sign upLogin
1
0

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?