Go to Qiita Advent Calendar 2024 Top
LoginSignup
2
1

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?

More than 1 year has passed since last update.

@jamjam(吉田 伸三)

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

Posted at

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

SDOの書き込み

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

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

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

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

2
1
0

Register as a new user and use Qiita more conveniently

  1. You get articles that match your needs
  2. You can efficiently read back useful information
  3. You can use dark theme
What you can do with signing up
Sign upLogin
2
1

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?