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++でプログラミング＜モータの回転 PPM＞

Last updated at Posted at 2022-06-07

　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

PPMの駆動手順

　EPOS4 Application Notesに上記の動作モードの手順が書かれています。「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) 
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で書き込みました。これらはコメントアウトしても、動作に影響はなかったです。

        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には必須なので書き込む関数が用意されています。

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

 VCS_SetEnableState(keyHandle, nodeId, &errorCode);

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

(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マッピングの設定ができていないと、不可解な動作をしました。

    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

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

2022-06-07 (2).png

プログラム

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

#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);
}
目次
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?