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

　　前回、オブジェクト・ディクショナリをSDOで読み出しました。

### SDOの書き込み

　位置決め制御で使われるPP（Profile Position Mode）では、まず、加速し、ある一定の速度を維持し、減速して目標の場所に到達する制御が行われます。ある一定の速度は、Profile velocity（目標速度）と呼ばれます。
　この値を読み出し、1000を書き込みます。単位はr/min=rpmです。


```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.RemoteNode(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")

velocity     = node.sdo[0x6081].raw
print('read Profile velocity     ',velocity)
print("")

node.sdo[0x6081].raw = 0x03e8  # Profile velocity 1000
velocity     = node.sdo[0x6081].raw
print('new Profile velocity     ',velocity)
print("") 

# Disconnect from CAN bus
network.disconnect()
```

　実行結果です。初期値は1でした。1000を書き込むと、1000が読み出されました。
![2022-03-03 (5).png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/407635/e473cec0-c75c-290c-f849-fefddf8b1c94.png)

### PDOの読み書きをするための状態を用意する
　SDOでは、オブジェクト・ディクショナリをすべて読み書きができます。その中で、特別に読み書きしたい項目をPDOマッピングします。CANopenでは100ms周期でデータが流れます。SDOはそういう同期的な通信ではありません。PDOで最新のデータを読み出せます。たとえば、現在の位置（Position actual value (6064h)）は、デフォルトでPDOにマッピングされています。また、どの位置に動かしたいかを指示するTarget positionもデフォルトでPDOにマッピングされています。
　CANopenでは**NMT**と、モータを利用するときは**CiA-402 State Machine**の2種類の状態管理します。
　NMTは電源が入るとInitialization状態になり、指示することで、Pre-operationalに移行します。この条件では、SDOによってオブジェクト・ディクショナリの読み書きができます。実働状態はOperationalで、PDOでリアルタイムにデータが読み書きされます。
　CiA-402 State Machineでは、Switch ON Disabled、Ready to Switch ON、Switched ON、Operation Enabled、Fault、Quick Stopなどの状態があり、Switched ON、Operation Enabled状態で、モータを回すことができます。過電流が流れたり、設定上限を超えたりするとFaultになります。

##### NMT

```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.RemoteNode(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('reset')
node.nmt.send_command(0x81)  # NMT reset
time.sleep(0.5)
network.check()
print('node state  = {0}'.format(node.nmt.state))

print('\nReset Communication')
node.nmt.send_command(0x82)  # NMT Reset Communication
node.nmt.wait_for_bootup(15)
network.check()
print('node state  = {0}'.format(node.nmt.state))

print('\nPre-ope')
node.nmt.send_command(0x80)  # NMT Pre-ope
network.check()
print('node state  = {0}'.format(node.nmt.state))

print('\nstart')
node.nmt.send_command(0x1)  # NMT start
network.check()
print('node state  = {0}'.format(node.nmt.state))
print("")

node.nmt.state = 'PRE-OPERATIONAL'
network.check()
print('node state  = {0}'.format(node.nmt.state))
print("")

node.nmt.state = 'OPERATIONAL'
network.check()
print('node state  = {0}'.format(node.nmt.state))
print("")

print("-----")
node.nmt.send_command(0x2)  # 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.disconnect()
```


　実行結果です。
　最初に、NMTのリセットをかけます。Initialization状態です。続いて、バス・エラーに対応するReset Communicationをかけます。PRE-OPERATIONAL状態です。
　明示的にPRE-OPERATIONAL状態にします。OPERATIONAL状態に移行します。
　明示的にPRE-OPERATIONAL状態に移行します。node.nmt.send_command(0x80) でもnode.nmt.state = 'PRE-OPERATIONAL'のどちらでも同じ指示が出せます。
　OPERATIONAL状態に移行します。node.nmt.send_command(0x1) でもnode.nmt.state = 'OPERATIONAL'のどちらでも同じ指示が出せます。

　最後は、node.nmt.send_command(0x2)でNMTがストップ状態になります。
　途中でエラーが起こった時を想定し、resetとReset Communicationをかけます。
　

![2022-03-03 (7).png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/407635/c09e1d07-195e-9158-38c1-5a75928bcd00.png)

##### CiA-402 State Machine
　状態は、0x6041のBits in Statuswordで読めます。ステータスは16ビットで構成されていますが、注目するのはLSBから3ビット分です。
- 0 Ready to Switch ON
- 1 Switched ON
- 2 Operation Enabled
- 3 Fault
- 4 Voltage Enabled
- 5 Quick Stop
- 6 Switch ON Disabled
- 7 Warning
- 8 Drive profile operation ready (manufacturer-specific (MS))
- 9 Remote
- 10 Target Reached
- 11 Internal Limit Active
- 12,13 Operation Mode Specific (OMS)
- 14 Reserved (manufacturer-specific (MS))
- 15 TLC (manufacturer-specific (MS))

```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('reset')
node.nmt.send_command(0x81)  # NMT reset
time.sleep(0.5)
network.check()
print('node state  = {0}'.format(node.nmt.state))

print('\nReset Communication')
node.nmt.send_command(0x82)  # NMT Reset Communication
node.nmt.wait_for_bootup(15)
network.check()
print('node state  = {0}'.format(node.nmt.state))

print('\nPre-ope')
node.nmt.send_command(0x80)  # NMT Pre-ope
network.check()
print('node state  = {0}'.format(node.nmt.state))

print('\nstart')
node.nmt.send_command(0x1)  # NMT start
network.check()
print('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)
print("(1)SWITCH ON DISABLED") 
node.state = 'SWITCH ON DISABLED'
time.sleep(0.5) 

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

print("(2)READY TO SWITCH ON") 
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.5) 

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

print("(3)SWITCH ON") 
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.5) 

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

print("(4)OPERATION ENABLED") 
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.5) 

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


print("\n<<<PDO read/write>>>\n")

time.sleep(1)
print("(end)QUICK STOP ACTIVE") 
node.state = 'QUICK STOP ACTIVE'
Statusword     = node.sdo[0x6041].raw
print('Statusword  {:4d}  0x{:x}  {:0>16b}'.format(Statusword,Statusword,Statusword))
print("") 
print("-----")
node.nmt.send_command(0x2)  # 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()
```

　nodeインスタンスのところではリモートからBaseNode402に変更しました。途中、network.sync.start(0.1)で100msのイベント同期を起動しています。
　node.setup_402_state_machine()はなくてもSWITCHの遷移は行えますが、6041にかかわるエラーがたくさんでます。
　QUICK STOPでSWITCHの稼働をOFFにします。
　Faultの解除、NMTのリセットなど、多めのエラー回復を挟んでいます。本来ならば、try文を入れて処理を記述すべきだと思います。

　実行結果です。
　OPERATION ENABLEDまで行くと、モータが鳴り始めます。
　``print("\n<<<PDO read/write>>>\n")``の部分で、実際にマッピングされたPDOの読み書きをします。
![2022-03-03 (10).png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/407635/9d0c5604-97e1-5165-a9e6-a5778ac3b0f3.png)