日記；回すだけIII ⑨ pythonでSDO、PDOの読み書き＜その4 Profile Position Mode（EPOS4）＞ベータ版

　前回までのオリエンタルモーターではなくmaxonのEPOS4＋100W ECモータを動かします。前回も、完全に制御できたとは言えませんが、今回はもっとダメダメです。
　STマイクロのキットについていたmaxonの約100Wのモータを使います。とても安く入手でき、1024cptのエンコーダもついています。EC-i 40と思われます。
　コントローラは、「EPOS4 Compact 24/1.5 CAN、 デジタル位置制御ユニット、1.5 A、10 - 24 VDC」で、とてもコンパクトです。コネクタ類はRSコンポーネントで入手し、組み立てました。電源コネクタがなぜかとても小さいです。

EPOS Studio 3.7でチューニング前の設定

　maxonモータはたくさんの品種があります。EPOS4やEPOS2のようにいくつかのコントローラもあります。コントローラは、Current controlとPosition controllerがあり、PI/PID制御が行われています。これらのゲインを適正に設定しないと軽やかに回転しませんし、目的の位置制御が不正確になります。最悪、目的位置で振動を起こし止まらなくなります。
　フリーのEPOS Studio 3.7ツールを使ってチューニングをします。
　WizerdsのStartupからパラメータを入れていきます。
　EC-i 40の諸元から写していきます。電流は1500mAだとチューニングのほとんどを失敗しました。500mAはチューニングできましたが、モータの能力的にもったいないので1000mAで行いました。つないでいる電源は24V/2.5Aです。

　ホール・センサがついているモータです。STマイクロのキットではこれにエンコーダが追加されています。

　制御の構成はデフォルトのままです。

　リミットの設定です。よくわからないので適当な値を入れています。あまり低い値だとチューニングですぐにフォルトになります。

　制御方法はデフォルトのままだったと思います。

　Windowsもデフォルトのままだと思います。

　Finishします。

　こちらに用語の解説ページがあります。

チューニング

　Regulation Tuningを選びます。電流から始まります。Auto tuneをクリックします。モータは激しく動くので、何らかの固定をしておかないと暴れます。

　速度です。

　ポジションです。Step amplitudeは1000incにしました。
　緑色が、設定位置と実際の場所のずれです。加速と減速時にずれを高速になおしているのがわかります。

　Parameter Export/Importから、Export Parameter to Fileの保存を選びます。DCFファイルで保存されます。ここでは使いません。チューニングされたパラメータはコントローラのEEPROMに保存されるようなので、同じものを複数使うような用途でDCFファイルは使われるようです。

　ToolsのObject dictionaryをダブルクリックして開きます。Indexの番号のてきとうな数字の上でマウスの右ボタンをクリックして出てくるメニューからExport EDS Fileをえらび、デスクトップに保存しておきます。プログラムの最初で読み込みます。

プログラム

　PPモードを動かすには、NMTを遷移させOPERATIONALにもっていきます。そして、CiA402のSwitchをONしてEnableまでもっていってからPPMのコマンドを送ります。しかし、NMTの操作は失敗します。CiA402の遷移も失敗します。

NMT

　このプログラム部分は前回と同じですが、実行は失敗します。

import canopen
import time

# Start with creating a network representing one CAN bus
network = canopen.Network()

# Add some nodes with corresponding Object Dictionaries
node = canopen.BaseNode402(5,'maxon_motor_EPOS4_0170h_6050h_0000h_0000h.eds')
network.add_node(node)

# Connect to the CAN bus
network.connect(bustype='ixxat', channel=0, bitrate=1000000)

print("\n===start  ID=5 EPOS4\n")

node.nmt.send_command(0x81)  # NMT reset
time.sleep(0.5)
node.nmt.send_command(0x82)  # NMT Reset Communication
#node.nmt.wait_for_bootup(15)
time.sleep(0.5)
node.nmt.send_command(0x80)  # NMT Pre-ope
time.sleep(0.5)
node.nmt.send_command(0x1)   # NMT start
network.check()
print('NMT:node state  = {0}\n'.format(node.nmt.state))
#--------------------

　インストールされたCANopenライブラリのclient.pyを開きます。
C:\Users\yoshidawin\AppData\Local\Packages\PythonSoftwareFoundation.Python.3.9_qbz5n2kfra8p0\LocalCache\local-packages\Python39\site-packages\canopen\sdo\client.py

class SdoClient(SdoBase):
    """Handles communication with an SDO server."""

    #: Max time in seconds to wait for response from server
    RESPONSE_TIMEOUT = 0.3

    #: Max number of request retries before raising error
    MAX_RETRIES = 3  ###1 to 3

　MAX_RETRIESを1から3に変更して保存します。下の実行した様子を示しますが、リトライの2回目で次へ進んでいます。場合によってはリトライ3回で何とか次へ進むようです。

CiA402のSwitch遷移

　この遷移も失敗します。いろいろなtimeout値を増やしてみたのですが、効果はありません。

network.sync.start(0.1)

#node.load_configuration()

print("---Switch CiA402") 
node.setup_402_state_machine()
#node.sdo[0x6040].raw = 0x0010  # Fault Reset
node.sdo[0x1003].raw = 0x0000
time.sleep(0.1)
node.sdo[0x1003].raw = 0x0000
time.sleep(0.1)
node.sdo[0x1003].raw = 0x0000
time.sleep(0.1)
#node.state = 'NOT READY TO SWITCH ON'

#node.state = 'SWITCH ON DISABLED'
time.sleep(0.1)  

Statusword = node.sdo[0x6041].raw
print('Statusword  {:0>16b}'.format(Statusword))

#timeout = time.time() + 15
#node.state = 'READY TO SWITCH ON'
node.sdo[0x6040].raw = 0x0006
time.sleep(0.1)
node.sdo[0x6040].raw = 0x0006
time.sleep(0.1)
node.sdo[0x6040].raw = 0x0006
time.sleep(0.1)

#while node.state != 'READY TO SWITCH ON':
#    if time.time() > timeout:
#        raise Exception('Timeout when trying to change state')
#    time.sleep(0.1)
#time.sleep(0.1)  
#timeout = time.time() + 15
#node.state = 'SWITCHED ON'
node.sdo[0x6040].raw = 0x0007
time.sleep(0.1)
node.sdo[0x6040].raw = 0x0007
time.sleep(0.1)
node.sdo[0x6040].raw = 0x0007
time.sleep(0.1)
#while node.state != 'SWITCHED ON':
#    if time.time() > timeout:
#        raise Exception('Timeout when trying to change state')
#    time.sleep(0.001)
#time.sleep(0.1)  
#timeout = time.time() + 15
#node.state = 'OPERATION ENABLED'
node.sdo[0x6040].raw = 0x000f
time.sleep(0.1)
node.sdo[0x6040].raw = 0x000f
time.sleep(0.1)
node.sdo[0x6040].raw = 0x000f
time.sleep(0.1)
#while node.state != 'OPERATION ENABLED':
#    if time.time() > timeout:
#        raise Exception('Timeout when trying to change state')
#    time.sleep(0.001)
#time.sleep(0.1) 
print("SWITCH ready\n") 

　前回のプログラムはコメントアウトし、Controlword（0x6040）に直接Fault Reset、READY TO SWITCH ON、SWITCHED ON、OPERATION ENABLEDのビットを3回ずつ書き込みました。
　実行すると、OPERATION ENABLEDまで遷移しました。SDOが遅くなるというメッセージが出ています。これは、node.setup_402_state_machine()の関数内で出ています。

　PDOマッピングはオーソドックスに四つです。そのsubは、オリエンタルモーターでは四つでしたが、EPOS4では12個あります。マッピングされている内容は、オリエンタルモーターとまったく同じです。

main

　CW、CCW、CWと回転するように記述しました。正しく動作しないこともあります。その時は、軸を手で少し回しておきます。
最初に、SDOで初期値を設定しています。
　最初のModes of operationを0x0001にしてPPモードにします。これは、後のPDOでも毎回設定しています。
　次のTarget positionはとりあえず0に、Profile velocityの速度は30ととても低速です。
　CW、CCW、CWでは、PDOでTarget positionは毎回変更していて反映されているように見えます。速度を上げるためにTarget velocityを大きな値にしているのですが無視され、30の低速のまま回ります。
　node.sdo[0x6081].raw = 30をコメントアウトすると、CW、CCW、CWのTarget velocityが有効になっているように観測できます。
　
node.sdo[0x6060].raw = 0x0001 # Modes of operation <- Profile Position Mode
node.sdo[0x607a].raw = 0 # Target position
node.sdo[0x6081].raw = 30 # Profile velocity

　node.rpdo[ｘ].start(0.1)とnode.rpdo[ｘ].stop()は、100msのSYNCに従って命令を発行します。前回のオリエンタルモーターでは、node.rpdo[ｘ].transmit()だけで動きましたが、EPOS4では必要な部分があるようです。
　start(0.1)してそのままにすると、100msごとにtransmitが実行されるようです。波形を見ていませんが、回転の動きからの推定です。正しい動作だと思います。したがって、1枚だけの実行を期待しているので、startした直後にstopさせます。
　現在位置の読み出しはSDOとPDOの両方で行っています。SDOは何らかの数値を読みますが、PDOでは0しか読めません。読み出しに何らかのトラブルを抱えているようです。

　RPDO communication parameterおよびTPDO communication parameter がNode-ID＝20の値がデフォルトで設定されているようです。ここでは5なので、変更しないといけません。修正版を後半に掲載しました

#----main----------------

Position_actual_value = node.sdo[0x6064].phys
Statusword = node.sdo[0x6041].raw

print('Position_actual_value ',Position_actual_value)
print('Statusword  {:0>16b}'.format(Statusword))

node.sdo[0x6060].raw = 0x0001  #  Modes of operation <- Profile Position Mode
node.sdo[0x607a].raw = 0       # Target position
node.sdo[0x6081].raw = 30      # Profile velocity
node.sdo[0x6083].raw = 5000    # Prfile acceleration


Position_actual_value = node.sdo[0x6064].phys
Statusword = node.sdo[0x6041].raw

node.tpdo.read()
print('new Target position {:4d}'.format(node.tpdo[3]['Position actual value'].phys))


node.nmt.state = 'OPERATIONAL'
node.rpdo.read()
node.rpdo[1]['Controlword'].phys  = 0x000F
node.rpdo[1].transmit()
node.rpdo[1].start(0.1)
node.rpdo[2]['Controlword'].phys  = 0x007f
node.rpdo[2]['Modes of operation'].phys  = 0x0001
node.rpdo[2].transmit()
#node.rpdo[2].start(0.1)
node.rpdo[3]['Controlword'].phys  = 0x007f
node.rpdo[3]['Target position'].phys  = 5000
node.rpdo[3].transmit()
node.rpdo[3].start(0.1)
node.rpdo[4]['Controlword'].phys  = 0x007f
node.rpdo[4]['Target velocity'].phys  = 500
node.rpdo[4].transmit()
node.rpdo[4].transmit()
#node.rpdo[4].start(0.1)


node.rpdo[1].stop()
#node.rpdo[2].stop()
node.rpdo[3].stop()
node.rpdo[4].stop()
time.sleep(2)

node.rpdo.read()
node.rpdo[1]['Controlword'].phys  = 0x0000f
node.rpdo[1].transmit()
node.rpdo[1].start(0.1)
node.rpdo[2]['Controlword'].phys  = 0x007f
node.rpdo[2]['Modes of operation'].phys  = 0x0001
node.rpdo[2].transmit()
#node.rpdo[2].start(0.1)
node.rpdo[3]['Controlword'].phys  = 0x007f
node.rpdo[3]['Target position'].phys  = -5000
node.rpdo[3].transmit()
node.rpdo[3].start(0.1)
node.rpdo[4]['Controlword'].phys  = 0x007f
node.rpdo[4]['Target velocity'].phys  = 2500
node.rpdo[4].transmit()
node.rpdo[4].start(0.1)


node.rpdo[1].stop()
#node.rpdo[2].stop()
node.rpdo[3].stop()
node.rpdo[4].stop()
time.sleep(3)


node.tpdo.read()
print('new Target position {:4d}'.format(node.tpdo[3]['Position actual value'].phys))
position0 = node.tpdo[3]['Position actual value'].phys
print(position0)
print('Statusword {:0>16b}'.format(Statusword))

node.rpdo.read()
node.rpdo[1]['Controlword'].phys  = 0x000F
node.rpdo[1].transmit()
node.rpdo[1].start(0.1)
node.rpdo[2]['Controlword'].phys  = 0x007f
node.rpdo[2]['Modes of operation'].phys  = 0x0001
node.rpdo[2].transmit()
#node.rpdo[2].start(0.1)
node.rpdo[3]['Controlword'].phys  = 0x007f
node.rpdo[3]['Target position'].phys  = 5000
node.rpdo[3].transmit()
node.rpdo[3].start(0.1)
node.rpdo[4]['Controlword'].phys  = 0x007f
node.rpdo[4]['Target velocity'].phys  = 5000
node.rpdo[4].transmit()
node.rpdo[4].start(0.1)

node.rpdo[1].stop()
#node.rpdo[2].stop()
node.rpdo[3].stop()
node.rpdo[4].stop()
time.sleep(2)

Position_actual_value = node.sdo[0x6064].phys
Statusword = node.sdo[0x6041].raw
print('\nPosition_actual_value ',Position_actual_value)
print('Statusword {:0>16b}'.format(Statusword))


#-------------------
print("\n-----")
node.sdo[0x6040].raw = 0x0010  # Fault Reset
print("") 
node.nmt.send_command(0x02)  # NMT remote stop
network.check()
print('node state end) = {0}'.format(node.nmt.state))
node.nmt.send_command(0x81)  # NMT reset
time.sleep(0.5)
node.nmt.send_command(0x82)  # NMT Reset Communication
network.sync.stop()
network.disconnect()

　終了処理部分は、前回と同じです。特にLEDの点滅が異常になっていないので、正しく動いているようです。

修正

PDOマッピング

　PDOのマッピングは、次のアドレスになります。

Receive-PDO (RPDO) (Master -> Driver)
- 1400h: 1st RPDO communication parameter
- 1401h: 2nd RPDO communication parameter
- 1402h: 3rd RPDO communication parameter
- 1403h: 4th RPDO communication parameter
- 1600h: 1st RPDO mapping parameter
- 1601h: 2nd RPDO mapping parameter
- 1602h: 3rd RPDO mapping parameter
- 1603h: 4th RPDO mapping parameter
Transmit-PDO (TPDO) (Driver -> Master)
- 1800h: 1st TPDO communication parameter
- 1801h: 2nd TPDO communication parameter
- 1802h: 3rd TPDO communication parameter
- 1803h: 4th TPDO communication parameter
- 1A00h: 1st TPDO mapping parameter
- 1A01h: 2nd TPDO mapping parameter
- 1A02h: 3rd TPDO mapping parameter
- 1A03h: 4th TPDO mapping parameter

communication parameter部分

　もともとのマッピングです。

Receive-PDO (RPDO) (Master -> Driver)
- 1400h: 1st RPDO communication parameter   0x01:0x00000220, 0x02:asyncronous
- 1401h: 2nd RPDO communication parameter  0x01:0x00000320, 0x02:asyncronous
- 1402h: 3rd RPDO communication parameter  0x01:0x00000420, 0x02:asyncronous
- 1403h: 4th RPDO communication parameter  0x01:0x00000520, 0x02:asyncronous

Transmit-PDO (TPDO) (Driver -> Master)
- 1800h: 1st TPDO communication parameter  0x01:0x400001a0, 0x02:asyncronous
- 1801h: 2nd TPDO communication parameter  0x01:0xc00002a0, 0x02:asyncronous
- 1802h: 3rd TPDO communication parameter  0x01:0xc00003a0, 0x02:asyncronous
- 1803h: 4th TPDO communication parameter  0x01:0xc00004a0, 0x02:asyncronous

　これを次のように修正します。WizardsからCANopenをクリックします。
　Receive-PDOの画面を出します。
　COB-IDは0x200＋Node-IDなので、0x00000205、0x00000305、0x00000405、0x00000505に変更します。asyncronousは全部synchronousに変更します。
　RxPDO4はTarget velocityだったので、ｘで消し、右からProfile velocity（6081）を追加します。これは4th RPDO mapping parameterの変更になります。mapping parameterの変更はここだけです。

　Transmit-PDO画面を出します。
　COB-IDは0x180＋Node-IDなので、0x00000185、0x00000285、0x00000385、0x00000485に変更します。asyncronousは全部synchronousに変更します。
　RTRのチェックを外し、Finishして再度開くと、COB-IDは0x40000xxxxに変更されています。

　EPOS4でこのように修正する解説は見つかっていません。EPOS2の解説（　https://cdn.vector.com/cms/content/know-how/_application-notes/canopen/AN-JON-1-701_Learning_CANopen_from_EPOS_and_CANsetter.pdf　）はありますが、選択できる項目が異なって参考になりません。Pythonのプログラムとの兼ね合いで変更しています。

プログラム

import canopen
import time

# Start with creating a network representing one CAN bus
network = canopen.Network()

# Add some nodes with corresponding Object Dictionaries
node = canopen.BaseNode402(5,'maxon_motor_EPOS4_0170h_6050h_0000h_0000h.eds')
network.add_node(node)

# Connect to the CAN bus
network.connect(bustype='ixxat', channel=0, bitrate=1000000)

print("\n===start  ID=5 EPOS4\n")

node.nmt.send_command(0x81)  # NMT reset
time.sleep(0.5)
node.nmt.send_command(0x82)  # NMT Reset Communication
#node.nmt.wait_for_bootup(15)
time.sleep(0.5)
node.nmt.send_command(0x80)  # NMT Pre-ope
time.sleep(0.5)
node.nmt.send_command(0x1)   # NMT start
network.check()
print('NMT:node state  = {0}\n'.format(node.nmt.state))
#--------------------

network.sync.start(0.1)

print("---Switch CiA402") 
node.setup_402_state_machine()

#node.sdo[0x6040].raw = 0x0010  # Fault Reset
node.sdo[0x1003].raw = 0x0000
time.sleep(0.1)
node.sdo[0x1003].raw = 0x0000
time.sleep(0.1)
node.sdo[0x1003].raw = 0x0000
time.sleep(0.1)
#node.state = 'NOT READY TO SWITCH ON'

#node.state = 'SWITCH ON DISABLED'

Statusword = node.sdo[0x6041].raw
print('Statusword  {:0>16b}'.format(Statusword))


#node.state = 'READY TO SWITCH ON'
node.sdo[0x6040].raw = 0x0006
time.sleep(0.1)
node.sdo[0x6040].raw = 0x0006
time.sleep(0.1)
node.sdo[0x6040].raw = 0x0006
time.sleep(0.1)
 
#node.state = 'SWITCHED ON'
node.sdo[0x6040].raw = 0x0007
time.sleep(0.1)
node.sdo[0x6040].raw = 0x0007
time.sleep(0.1)
node.sdo[0x6040].raw = 0x0007
time.sleep(0.1)

#node.state = 'OPERATION ENABLED'
node.sdo[0x6040].raw = 0x000f
time.sleep(0.1)
node.sdo[0x6040].raw = 0x000f
time.sleep(0.1)
node.sdo[0x6040].raw = 0x000f
time.sleep(0.1)

print("\nSWITCH ready\n") 
print('Statusword  {:0>16b}\n'.format(node.sdo[0x6041].raw))

#----main----------------

def move(position, velocity):
    node.sdo[0x6040].raw = 0x0006
    node.sdo[0x6040].raw = 0x000f
    node.rpdo[3]['Controlword'].phys  = 0x007f
    node.rpdo[3]['Target position'].phys  = position
    node.rpdo[3].transmit()
    node.rpdo[4]['Controlword'].phys  = 0x007f
    node.rpdo[4]['Profile velocity'].phys  = velocity
    node.rpdo[4].transmit()

def readPosition():
    print('Target position {:5d}'.format(node.tpdo[3]['Position actual value'].raw))

readPosition()

node.sdo[0x6060].raw = 0x0001  #  Modes of operation <- Profile Position Mode
node.sdo[0x6083].raw = 5000    # Prfile acceleration
    
node.nmt.state = 'OPERATIONAL'
print('\n>>> move\n')
move(15000,1000)

time.sleep(3)
readPosition()

print('\n>>> move\n')
move(20000,2500)

time.sleep(3)

readPosition()

print('\n>>> move\n')
move(-35000,300)

time.sleep(6)
readPosition()

#-------------------
print("\n-----")
node.sdo[0x6040].raw = 0x0010  # Fault Reset
print("") 
node.nmt.send_command(0x02)  # NMT remote stop
network.check()
print('node state end) = {0}'.format(node.nmt.state))
node.nmt.send_command(0x81)  # NMT reset
time.sleep(0.5)
node.nmt.send_command(0x82)  # NMT Reset Communication
network.sync.stop()
network.disconnect()