日記；回すだけIII ⑦ pythonでSDO、PDOの読み書き＜その2 Home＞ベータ版

　モータ・コントローラでは様々な動作モードを備えています。その中でも次の三つは基本機能です。

• Home；Homing method
• PPモード；Profile Position Mode　位置制御モード
• PVモード；Profile Velocity Mode　速度制御モード

Homing method

　モータ制御機によって実装されているホーミング（原点復帰？）機能は様々です。多くは、リミット・スイッチの信号が入ると原点に向かって移動する、という動作になっています。移動する方向などで、いくつかのHoming methodが用意されています。
　実験では、とくに物理的なリミット・スイッチで制限することはないので、実例は、今の場所を原点とするモードを実行します。
　Homing methodは35もしくは37の Homing on current positionを指定します。SDOで0x6098に書き込みます。
　Controlword of the Homing Modeでは、ビット4 HOS；Homing operation startのフラグがあり、1: Starts or continues homing procedureとなっています。しかし、TMCM-CANopenで実行したときは、最初は0が書き込まれていました。Modes of operation= 6を書き込んだ後、0x001fがControlwordに書き込まれています。

Tx: 0x60a: 40 60 60 00 00 00 00 00
Rx: 0x58a: 4f 60 60 00 03 00 00 00
Tx: 0x60a: 2f 60 60 00 06 00 00 00
Rx: 0x58a: 60 60 60 00 00 00 00 00
Tx: 0x60a: 40 40 60 00 00 00 00 00
Rx: 0x58a: 4b 40 60 00 0f 00 00 00
Tx: 0x60a: 2b 40 60 00 1f 00 00 00
Rx: 0x58a: 60 40 60 00 00 00 00 00

プログラム①

　単にModes of operation= 6を書き込んだ後、001fをControlwordで書き込んでも、原点復帰が行われない場合がありました。二度実行すると原点復帰します。想像ですが、このHoming methodは、原点に復帰が完了するまで、Modes of operation= 6を継続する必要があるのかもしれません。　　
　transmit()したのち、start(0.1)を実行すると、1回の実行で、位置が原点の0（付近）に変更になります。

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(10,'BLVD-KRD_CANopen_V100.eds')
network.add_node(node)

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

print("\n===start  ID=10 OrientalMotor\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)
node.nmt.send_command(0x80)  # NMT Pre-ope
node.nmt.send_command(0x1)   # NMT start
network.check()
print('NMT:node state  = {0}'.format(node.nmt.state))
print("")

#--------------------

network.sync.start(0.1)
print("---Switch CiA402 \n") 
node.setup_402_state_machine()
node.sdo[0x6040].raw = 0x0010  # Fault Reset
time.sleep(0.5)
node.state = 'SWITCH ON DISABLED'
time.sleep(0.1)  
timeout = time.time() + 15
node.state = 'READY TO SWITCH ON'
while node.state != 'READY TO SWITCH ON':
    if time.time() > timeout:
        raise Exception('Timeout when trying to change state')
    time.sleep(0.01)
time.sleep(0.1)  
timeout = time.time() + 15
node.state = 'SWITCHED ON'
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'
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") 

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

node.sdo[0x6098].raw = 35  #  Homing on current position

print("\n read this position")
node.nmt.state = 'OPERATIONAL'
node.tpdo.read()
node.tpdo[3].wait_for_reception()
position0 = node.tpdo[3]['Position actual value'].phys
print(position0)

node.rpdo[2]['Controlword'].phys  = 0  # Homing operation start
node.rpdo[2]['Modes of operation'].phys  = 6  # Homing Mode
node.rpdo[2].transmit()
node.rpdo[2].start(0.1)
node.rpdo[1]['Controlword'].phys  = 0x001f
node.rpdo[1].transmit()

time.sleep(1)

node.nmt.state = 'OPERATIONAL'
print("\n read new position")
node.tpdo.read()
node.tpdo[3].wait_for_reception()
position0 = node.tpdo[3]['Position actual value'].phys
print(position0)

node.sdo[0x6040].raw = 0x0010  # Fault Reset
#--------------------
print("")
print("-----")
node.nmt.send_command(0x02)  # NMT remote stop
network.check()
print('NMT: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()

　実行結果です。
　position0 = node.tpdo[3]['Position actual value'].physで現在位置を読み出し、homingを実行した後にも同じく現在位置を読み出しています。本来0になるはずですが、数単位ずれます。

プログラム②

　Statusword of the Homing ModeのHoming attainedをチェックするのが正しいかもしれません。ソースを読むと、対応した関数homing()が見つかりました。

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

print("\n read this position")
node.nmt.state = 'OPERATIONAL'
node.tpdo.read()
node.tpdo[3].wait_for_reception()
position0 = node.tpdo[3]['Position actual value'].phys
print(position0)

node.homing(35)
print("\n homimg")

node.nmt.state = 'OPERATIONAL'
print("\n read new position")
node.tpdo.read()
node.tpdo[3].wait_for_reception()
position0 = node.tpdo[3]['Position actual value'].phys
print(position0)

node.sdo[0x6040].raw = 0x0010  # Fault Reset
#--------------------

　実行すると、正しく0になります。ただこの関数、引数に何を入れても0になります。

PDOマッピング情報

　何がマッピングされているかは、マニュアルに書かれています。自分でマッピングする内容は書き換えられます。オブジェクト・ディクショナリにマッピング可能がどうかが書かれています。

Mapping

　マッピングされているアドレスです。

Receive-PDO (RPDO) (Master -> Driver)

• 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)

• 1A00h: 1st TPDO mapping parameter
• 1A01h: 2nd TPDO mapping parameter
• 1A02h: 3rd TPDO mapping parameter
• 1A03h: 4th TPDO mapping parameter

デフォルトのMapping

1600h: １個，6040 0010h、０，０，０　　　　　　Controlword
1601h: ２個，6040 0010h、6060 0008h，０，０　　Controlword、Modes of operation
1602h: ２個，6040 0010h、607A 0020h，０，０　　Controlword、Target position
1603h: ２個，6040 0010h、60FF 0020h，０，０　　Controlword、Target velocity

1A00h: １個，6041 0010h、０，０，０　　　　　　Statusword
1A01h: ２個，6041 0010h、6061 0008h，０，０　　Statusword、Modes of operation display
1A02h: ２個，6041 0010h、6064 0020h，０，０　　Statusword、Position actual value
1A03h: ２個，6041 0010h、606C 0020h，０，０　　Statusword、Velocity actual value