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

　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では、入力すべきパラメータの図が書かれています。
それらを読み出します。

```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(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なのか設定されてないのか判別できません。

![2022-03-06 (2).png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/407635/d93f206b-b2a7-bbdd-72a4-d3ac0af0012d.png)

　モータ・コントローラは、上記のように、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回目は、思ったような位置への回転ができていません。


### プログラム

```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(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()
```