前回のプログラムcanppm.pyの一部をを関数化、整理しました。プログラム名はcanppm2.pyです。位置決め部分をmove()という関数にしたので、mainのプログラムが見通しが良くなりました。
mainの最初で、
#resetNmt()
node.nmt.send_command(0x1) # NMT start
NMTのリセットをせずに、NMTをスタートさせるだけに変更しました。これは、MEXE02の接続が切れないための対策です。
|
import canopen
import time
import logging
# logging.basicConfig(level=logging.DEBUG)
# Start with creating a network representing one CAN bus
network = canopen.Network()
# Connect to the CAN bus
# sudo ip link set can0 up type can bitrate 1000000
network.connect(bustype='socketcan', channel='can0')
print("\n===start ID=12 OrientalMotor===\n")
# Add some nodes with corresponding Object Dictionaries
node = canopen.BaseNode402(12,'Downloads/BLVD-KRD_CANopen_V200.eds')
network.add_node(node)
def resetNmt():
# all nodes simulaneously as a broadcast message
network.nmt.state = 'RESET'
time.sleep(0.5)
# Reset this node
node.nmt.state = 'RESET'
node.nmt.wait_for_bootup(15)
node.nmt.state = 'RESET COMMUNICATION'
node.nmt.wait_for_bootup(15)
node.nmt.send_command(0x1) # NMT start
network.check()
print('---node state = {0}\n'.format(node.nmt.state))
def startState_machine():
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 state ready\n")
network.check()
print('---switch state = {0}\n'.format(node.state))
def move(targetPosition):
node.rpdo[1]['Controlword'].phys = 0x000f
node.rpdo[1].transmit()
time.sleep(0.02)
node.rpdo[3]['Controlword'].phys = 0x005f
node.rpdo[3]['Target position'].phys = targetPosition
node.rpdo[3].transmit()
time.sleep(0.02)
def dispParameter():
Modes_of_Operation = node.sdo[0x6060].raw
print('#Modes of Operation',Modes_of_Operation)
Modes_of_Operation_Display = node.sdo[0x6061].raw
print('#Modes of Operation Display',Modes_of_Operation_Display,'\n')
Target_Position = node.sdo[0x607a].raw
print('#Target Position',Target_Position)
Target_Velocity = node.sdo[0x60ff].raw
print('#Target Velocity', Target_Velocity,'\n')
Position_Actual_Value = node.sdo[0x6040].raw
print('#Position Actual Value',Position_Actual_Value)
Velocity_Actual_Value = node.sdo[0x606c].raw
print('#Velocity Actual Value', Velocity_Actual_Value,'\n')
Profile_velocity = node.sdo[0x6081].raw
print('#Profile velocity',Profile_velocity)
Profile_acceleration = node.sdo[0x6083].raw
print('#Profile acceleration',Profile_acceleration)
Profile_deceleration = node.sdo[0x6084].raw
print('#Profile deceleration', Profile_deceleration,'\n')
def dispPosition():
node.tpdo[3].wait_for_reception()
print('Target position {:4d}'.format(node.tpdo[3]['Position actual value'].phys))
#----main----------------
#resetNmt()
node.nmt.send_command(0x1) # NMT start
network.sync.start(0.1) # 100ms
startState_machine()
time.sleep(0.2)
#node.sdo[0x6060].raw = 0x0001 # Modes of operation <- Profile Position Mode
node.sdo[0x607a].raw = 0 # Target position
node.sdo[0x6081].raw = 3000 # Profile velocity
node.sdo[0x6083].raw = 1000 # Profile acceleration
node.sdo[0x6084].raw = 1000 # Profile deceleration
dispParameter()
dispPosition()
print(' ===start move===\n')
move(50000)
time.sleep(3)
dispPosition()
move(100000)
time.sleep(3)
dispPosition()
move(-150000)
time.sleep(3)
dispPosition()
#-------------------
print("\n---closed---")
node.sdo[0x6040].raw = 0x0080 # Fault Reset
node.nmt.send_command(0x02) # NMT remote stop
node.nmt.send_command(0x81) # NMT reset
time.sleep(0.5)
node.nmt.send_command(0x82) # NMT Reset Communication
network.sync.stop()
network.disconnect()
|
実行した様子です。
●減速を変える
回転を実施して読み出したTarget positionがきっちした値になっていません。3秒待ってというのも根拠がないのですが、ほぼ、このぐらいの回転数だと落ち着いた位置になっているから採用しています。本来なら、目標位置に到達したというのをステータスのいずれかのビットで判断するというのが正しいはずですが、うまい事例が見つかっていません。
減速の加速度がおおきすぎたのではないかと思い、500、200、50と変更して実行しました。500と200は少し最後の位置が0に近くなりましたが、50ぐらいの誤差があります。20では逆に200ぐらいにずれが多くなりました。
Profile deceleration 1000
では、Profile velocity 3000を1000と低速にしましたが、改善されません。
0に近づけるには、別な複雑な組み合わせがあるのかもしれません。
◆関連があるかもしれない
|
Position window (6067h) （現在の設定値 18）
現在の目標位置（Target position +（Position offset）に対して、現在位置（Position actual value）が到達したとみなす位置の差を設定します。位置エンコーダの実際の値がこの位置ウィンドウ内にある場合、目標位置に到達したと見なされます。
|
初期設定時に次の1行を追加して設定を変更してみました。
node.sdo[0x6067].raw = 6 # Position window default:18
実行しても、ほとんど位置に変化はありませんでした。
◆ちょっと関連があるかもしれないが実験していない
|
Following error window (6065h)
Position demand value (6062h)と Position actual value (6064h)の差に対して、Statusword (6041h)
の Following error (bit 13)を 1 にする閾値を設定します。単位はユーザー指定単位です。
|
●実行時に軸を手でつまむ
動作時に、軸を手でつまんで負荷がかかるようにしました。main部分のプログラムを修正して、連続的に0->30000->-30000とTarget positionを変化させています。resetNmt()を実行すると、MEXE02の通信が止まってしまうので、node.nmt.send_command(0x1) # NMT startだけを実行しました。
回転しているとき、軸を手でつまむと、トルクに変動が見られます。
位置、速度に関してPID制御が、トルクに対してもPID制御が行われていて、負荷が変動しても、目標位置へ正しく到達できるようになっています。
EPOS4では、それらを制御するPやIのgainを含めた計算方法が、EPOS4 Application NotesのCONTROLLER ARCHITECTUREに解説があります。PやIを設定するオブジェクト・ディクショナリのindexも用意されています。ツールには、チューニングするソフトも用意されています。
コントローラBLVD-KRDのオブジェクト・ディクショナリには、それらのindexはありません。
EPOS4では、モータの種類、ギアの種類、エンコーダの種類が多く、組み合わせたときの性能が予測できないぐらい多岐にわたります。なので、チューニング・ツールで、適切なP、I、Dのgainなどを設定しないとうまく動かず、振動が出たり、電流が流れすぎたりしてしまいます。
「BLMR5100K-A-B」 + 「BLVD-KRD」では、エンコーダのスペックも公表されていません。ギアは選択できます。それらを含めてチューニング済みの製品だと考えられます。
|
#----main----------------
#resetNmt()
node.nmt.send_command(0x1) # NMT start
network.sync.start(0.1) # 100ms
startState_machine()
time.sleep(0.2)
#node.sdo[0x6060].raw = 0x0001 # Modes of operation <- Profile Position Mode
node.sdo[0x607a].raw = 0 # Target position
node.sdo[0x6081].raw = 3000 # Profile velocity
node.sdo[0x6083].raw = 1000 # Profile acceleration
node.sdo[0x6084].raw = 1000 # Profile deceleration
dispParameter()
dispPosition()
print(' ===start move===\n')
for i in range(10):
move(30000)
time.sleep(0.1)
dispPosition()
move(-30000)
time.sleep(0.1)
dispPosition()
|
MEXE02の(m16) トレースモニタ機能を用いて動きを見ています。
たとえば、搬送車などを作ったときに、上に乗せる荷物の分量によってどういう変動をしているかなどを確認できそうです。
