Profile Position Modeは、設定したプロファイル（加速度、等速度、減速度）に沿って、指令位置まで移動するPTP（Point to Point）制御です。位置決めに使われます。
Profile Position Modeで使われるパラメータ
BLV シリーズ R タイプのBLV Series R Type Driver CANopen Communication Profile「HP-5143」のProfile Position Modeでは、入力すべきパラメータの図が書かれています。
それらを読み出します。
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")
print('\n@@need PP setting parameter')
Max_torqe = node.sdo[0x6072].raw
print('Max torqe',Max_torqe)
Target_position = node.sdo[0x607a].raw
print('Target position',Target_position)
Software_position_limit_min = node.sdo[0x607d][1].raw
print('Software position limit min',Software_position_limit_min)
Software_position_limit_max = node.sdo[0x607d][2].raw
print('Software position limit max',Software_position_limit_max)
Positioning_option_code = node.sdo[0x60f2].raw
print('Positioning option code',Positioning_option_code)
Profile_velocity = node.sdo[0x6081].raw
print('Prfile velocity',Profile_velocity)
End_velocity = node.sdo[0x6082].raw
print('End velocity ',End_velocity )
Profile_acceleration = node.sdo[0x6083].raw
print('Prfile acceleration',Profile_acceleration)
Profile_deceleration = node.sdo[0x6084].raw
print('Profile deceleration ',Profile_deceleration )
Quick_stop_deceleration = node.sdo[0x6085].raw
print('Quick stop deceleration',Quick_stop_deceleration)
Quick_stop_option_code = node.sdo[0x605a].raw
print('Quick stop option code',Quick_stop_option_code)
Halt_option_code = node.sdo[0x605d].raw
print('Halt option code',Halt_option_code)
Position_window = node.sdo[0x6067].raw
print('Position window',Position_window)
Following_error_window = node.sdo[0x6065].raw
print('Following error window',Following_error_window)
print("------")
実行結果です。0は、0なのか設定されてないのか判別できません。
モータ・コントローラは、上記のように、PPモードで目標位置のTarget position、台形駆動時の立ち上がり加速度Profile acceleration、一定速度になった時の速度Prfile velocity、減速時のProfile decelerationを指定し、そのほかに各種リミットなどを指定すれば回転します。
これ以外に、台形駆動はPID制御されているので、ゲインなどを適切に指定しなければ、振動したり、モータの能力を発揮できません。オリエンタルモーターは、コントローラとモータが一組しかないので、すでにEDSファイルに適切な値が初期値として記録されていると思われます。なので、特に変更はしません。
コントローラとモータの組み合わせがたくさんあるmaxonなどは、ツールを使ってチューニング作業を行い、DCFというEDSとよく似たXMLのファイルを出力し、それを初期値として読み込んで利用する形になっているようです。
PDOを詳しく知る
前回マッピングされたPDOのリストを載せました。制御に必要なのは、RPDO（Receive-PDO ；Master -> Driver)です。
- 1600h: 1st RPDO: １個，6040 0010h、０，０，０ Controlword
- 1601h: 2nd RPDO: ２個，6040 0010h、6060 0008h，０，０ Controlword、Modes of operation
- 1602h: 3rd RPDO: ２個，6040 0010h、607A 0020h，０，０ Controlword、Target position
- 1603h: 4th RPDO: ２個，6040 0010h、60FF 0020h，０，０ Controlword、Target velocity
Controlwordに指定できるモードを次に示します。
- 0x0006 ： Shutdown
- 0x0007 ： Disable
- 0x000B ： QuickStop
- 0x000F ： Enable
- 0x001F ： Absolute Positioning
- 0x003F ： Absolute Positioning Start Immediately
- 0x007F ： Relative Positioning
- 0x005F ： Relative Positioning Start Immediately
四つのRPDOに、上記のどれを指定すれば動かせるのかがよくわかりませんでした。
SDOで初期値を入れる
PDOのTarget positionを指定すれば回転すると思ったのですが、動きません。
- Modes of operation（0x6060）にProfile Position Modeの0x0001
- Target position（0x607a）に任意の数値
- Profile velocity（0x6081）に任意の数値
を、SDOで事前に値を入れておく必要があるようです。
mainの最初で、現在位置をRPDO[3]から取得します。上記の三つの値をSDOで書き込みます。
そのあと3回、回転をしています。CWを2回、最後はCCWです。それぞれ目標位置に到達するには時間がかかりますが、canopenライブラリで、到達したフラグ（Target reachedなど）を得る関数が見つかっていないので、time.sleep()で2，3秒待っています。
それぞれ、止まったときに、現在位置を読み出しています。
三つのブロックは、四つのRPDOの設定を行っています。1番目はControlwordだけですが、2～4番目は、二つ目の設定値があります。
1番目のControlwordには0x000fのenableモードを指定しています。内容は、Switch on、Enable voltage、Quick stop、Enable operationの4ビットを'1'にしています。2～4番目のControlwordはRelative Positioning Start Immediately（今の場所から目標値分回転する）の0x005fを指定しました。
2～4番目の二つ目の設定値は、それぞれ、Modes of operation=0x0001（Profile Position Mode）、Target position（目標）、Target velocity（速度）です。
3回の回転をしていますが、1回目は、思ったような位置への回転ができていません。
プログラム
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)
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
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.tpdo[3].wait_for_reception()
print('Statusword {:0>16b}'.format(node.tpdo[3]['Statusword'].phys))
orgPosition = node.tpdo[3]['Position actual value'].phys
print('org position {:4d}'.format(orgPosition))
node.sdo[0x6060].raw = 0x0001 # Modes of operation <- Profile Position Mode
node.sdo[0x607a].raw = orgPosition # Target position
node.sdo[0x6081].raw = 1000 # Profile velocity
node.nmt.state = 'OPERATIONAL'
node.rpdo[1]['Controlword'].phys = 0x000f
node.rpdo[1].transmit()
node.rpdo[2]['Controlword'].phys = 0x005f
node.rpdo[2]['Modes of operation'].phys = 0x0001
node.rpdo[2].transmit()
node.rpdo[3]['Controlword'].phys = 0x005f
node.rpdo[3]['Target position'].phys = 20000
node.rpdo[3].transmit()
node.rpdo[4]['Controlword'].phys = 0x005f
node.rpdo[4]['Target velocity'].phys = 0x0aE8
node.rpdo[4].transmit()
time.sleep(2)
node.tpdo[3].wait_for_reception()
print('new Target position {:4d}'.format(node.tpdo[3]['Position actual value'].phys))
node.rpdo[1]['Controlword'].phys = 0x000f
node.rpdo[1].transmit()
node.rpdo[3]['Controlword'].phys = 0x005f
node.rpdo[3]['Target position'].phys = 10000
node.rpdo[3].transmit()
node.rpdo[4]['Controlword'].phys = 0x005f
node.rpdo[4]['Target velocity'].phys = 0x03E8 #max 4000000
node.rpdo[4].transmit()
time.sleep(2)
node.tpdo[3].wait_for_reception()
print('new Target position {:4d}'.format(node.tpdo[3]['Position actual value'].phys))
node.rpdo[1]['Controlword'].phys = 0x000f
node.rpdo[1].transmit()
node.rpdo[3]['Controlword'].phys = 0x005f
node.rpdo[3]['Target position'].phys = -50000
node.rpdo[3].transmit()
node.rpdo[4]['Controlword'].phys = 0x005f
node.rpdo[4]['Target velocity'].phys = 0x5C18
node.rpdo[4].transmit()
time.sleep(3)
node.tpdo[3].wait_for_reception()
print('new Target position {:4d}'.format(node.tpdo[3]['Position actual value'].phys))
#-------------------
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()