def main(): """メイン関数 処理内容1:運転データNo.0の位置の読み込み """ global gMotor_pos rospy.init_node("sample1_2", anonymous=True) pub = rospy.Publisher("om_query1", om_query, queue_size=1) # OMノードに送信するための定義 rospy.Subscriber("om_response1", om_response, resCallback) # レスポンスのコールバック定義 rospy.Subscriber("om_state1", om_state, stateCallback) # レスポンスのコールバック定義 msg = om_query() # ノードで定義されたメッセージを使用 time.sleep(1) # 読み込み(運転データNo.0の位置) msg.slave_id = 0x01 # 号機選択(Hex): 1号機 msg.func_code = 0 # ファンクションコード選択: 0(Read) msg.read_addr = 6146 # 先頭アドレス選択(Dec): 運転データNo.0の位置 msg.read_num = 1 # 読み込みデータサイズ: 1 (32bit) pub.publish(msg) # クエリ生成ノードに上記内容を送信。ノードでmsg作成後はドライバに送信 wait() # 処理待ち # 読み込んだ値を表示 print("position = {0:}[step]".format(gMotor_pos)) print("END") # 終了表示 rospy.spin()
def main(): """メイン関数 処理内容1:運転データNo.2の回転速度を書き込み """ rospy.init_node("sample1_1", anonymous=True) pub = rospy.Publisher("om_query1", om_query, queue_size=1) # OMにノードに送信するまでの定義 rospy.Subscriber("om_state1", om_state, stateCallback) # レスポンスのコールバック定義 msg = om_query() # ノードで定義されたメッセージを使用 time.sleep(1) # 書き込み(回転速度No.2) msg.slave_id = 0x01 # 号機選択(Hex): 1号機 msg.func_code = 1 # ファンクションコード選択: 1(Write) msg.write_addr = 1156 # 先頭アドレス選択(Dec): データNo.2 回転速度 msg.write_num = 1 # 書き込みデータサイズ: 1 (32bit) msg.data[0] = 123 # 回転速度[r/min] pub.publish(msg) # クエリ生成ノードに上記内容を送信。ノードでmsg作成後はドライバに送信 wait() # 処理待ち print("END") # 終了表示 rospy.spin()
def main(): """メイン関数 処理内容1:運転データNo.2の回転速度を書き込み 処理内容2:運転指令(FWD方向) 処理内容3:停止指令(減速停止) """ global gState_mes global gState_error MESSAGE_ERROR = 2 EXCEPTION_RESPONSE = 2 rospy.init_node("sample2", anonymous=True) pub = rospy.Publisher("om_query1", om_query, queue_size=1) # OMにノードに送信するまでの定義 rospy.Subscriber("om_state1", om_state, stateCallback) # レスポンスのコールバック定義 msg = om_query() # OMノードで作成したメッセージを使用 time.sleep(1) init(msg, pub) # 初期化関数のコール # 速度設定 # 運転データNo.2の回転速度を書き込み msg.slave_id = 0x01 # 号機選択(Hex): 1号機 msg.func_code = 1 # ファンクションコード選択: 1(Write) msg.write_addr = 1156 # 先頭アドレス選択(Dec): データNo.2 回転速度 msg.write_num = 1 # 書き込みデータサイズ: 1 (32bit) msg.data[0] = 1000 # 書き込みデータ: 1000[r/min] pub.publish(msg) # クエリ生成ノードに上記内容を送信。ノードでmsg作成後はドライバに送信 wait() # 処理待ち # 運転指令(FWD方向)(M1,START/STOP,RUN/BRAKEをON) msg.slave_id = 0x01 # 号機選択(Hex): 1号機 msg.func_code = 1 # ファンクションコード選択: 1(Write) msg.write_addr = 124 # 先頭アドレス選択(Dec): 動作コマンド msg.write_num = 1 # 書き込みデータサイズ: 1 (32bit) msg.data[0] = 26 # 書き込みデータ: ONビット(0000 0000 0001 1010) = 26 pub.publish(msg) # クエリ生成ノードに上記内容を送信。ノードでmsg作成後はドライバに送信 wait() # 処理待ち # メッセージエラーの発生 if (gState_mes == MESSAGE_ERROR): stop(msg, pub) exit() # 処理の強制終了 # 例外応答の発生 if (gState_error == EXCEPTION_RESPONSE): stop(msg, pub) exit() # 処理の強制終了 # 5秒待機 time.sleep(5) # 停止指令 stop(msg, pub) print("END") # 終了表示 rospy.spin() # msg取得用関数の実行(必須)
def main(): """メイン関数 処理内容1:運転データを書き込み 処理内容2:運転開始(START信号ON) """ global gState_mes global gState_error MESSAGE_ERROR = 2 EXCEPTION_RESPONSE = 2 rospy.init_node("sample2_1", anonymous=True) pub = rospy.Publisher("om_query1", om_query, queue_size=1) # OMノードに送信するための定義 rospy.Subscriber("om_state1", om_state, stateCallback) # レスポンスのコールバック定義 msg = om_query() # OMノードで作成したメッセージの保存 time.sleep(1) # 運転データを書き込み msg.slave_id = 0x01 # 号機選択(Hex): 1号機 msg.func_code = 1 # ファンクションコード選択: 1(Write) msg.write_addr = 6144 # 先頭アドレス選択(Dec): 運転データNo.0の方式(1800h) msg.write_num = 6 # 書き込みデータサイズ: 6 (6x32bit) msg.data[0] = 2 # 方式: 2:相対位置決め(指令位置基準) msg.data[1] = 5000 # 位置: 5000[step] msg.data[2] = 1000 # 速度: 1000[Hz] msg.data[3] = 1000 # 起動・変速レート: 1.0[kHz/s] msg.data[4] = 1000 # 停止レート: 1.0[kHz/s] msg.data[5] = 1000 # 運転電流: 100[%] pub.publish(msg) # クエリ生成ノードに上記内容を送信。ノードでmsg作成後はドライバに送信 wait() # 処理待ち # 運転開始 msg.slave_id = 0x01 # 号機選択(Hex): 1号機 msg.func_code = 1 # ファンクションコード選択: 1(Write) msg.write_addr = 124 # 先頭アドレス選択(Dec): ドライバ入力指令 msg.write_num = 1 # 書き込みデータサイズ: 1 (32bit) msg.data[0] = 8 # 書き込みデータ: (0000 0000 0000 1000) = 8(START信号ON) pub.publish(msg) # クエリ生成ノードに上記内容を送信。ノードでmsg作成後はドライバに送信 wait() # 処理待ち # メッセージエラーの発生 if (gState_mes == MESSAGE_ERROR): stop(msg, pub) # 運転停止 exit() # 処理の強制終了 # 例外応答の発生 if (gState_error == EXCEPTION_RESPONSE): stop(msg, pub) # 運転停止 exit() # 処理の強制終了 time.sleep(5) # 5秒待機 stop(msg, pub) # 運転停止 print("END") # 終了表示 rospy.spin() # msg取得用関数の実行(必須)
def main(): """メイン関数 処理内容1:軸1、軸2にダイレクトデータ運転を書き込み(相対位置決め運転) 処理内容2:検出位置の読み込み(LOOP:5回) """ global gMotor_pos1 global gMotor_pos2 rospy.init_node("sample3", anonymous=True) pub = rospy.Publisher("om_query1", om_query, queue_size=1) # OMノードに送信するための定義 rospy.Subscriber("om_state1", om_state, stateCallback) # レスポンスのコールバック定義 rospy.Subscriber("om_response1", om_response, resCallback) # レスポンスのコールバック定義 msg = om_query() # OMノードで作成したメッセージの保存 time.sleep(1) init(msg, pub) # 初期化関数のコール r = rospy.Rate(1) # ループ周期1秒 print("START") for i in range(5): # LOOP処理(5回) # 指令位置の書き込み msg.slave_id = 0x01 # 号機選択(Hex): 1 msg.func_code = 1 # ファンクションコード選択: 1(Write) msg.write_addr = 88 # 先頭アドレス選択(Dec): ダイレクト運転データNo.(0058h) msg.write_num = 8 # 書き込みデータサイズ: 8(8x32bit) msg.data[0] = 0 # 運転データNo.: 0 msg.data[1] = 2 # 方式: 2:相対位置決め(指令位置基準) msg.data[2] = 100 # 位置: 100[step] msg.data[3] = 2000 # 速度: 2000[Hz] msg.data[4] = 2000 # 起動・変速レート: 2.0[kHz/s] msg.data[5] = 2000 # 停止レート: 2.0[kHz/s] msg.data[6] = 1000 # 運転電流: 100[%] msg.data[7] = 1 # 反映トリガ: 1(全データ反映) pub.publish(msg) # クエリ生成ノードに上記内容を送信。ノードでmsg作成後はドライバに送信 wait() msg.slave_id = 0x02 # 号機選択(Hex): 2 msg.func_code = 1 # ファンクションコード選択: 1(Write) msg.write_addr = 88 # 先頭アドレス選択(Dec): ダイレクト運転データNo.(0058h) msg.write_num = 8 # 書き込みデータサイズ: 8(8x32bit) msg.data[0] = 0 # 運転データNo.: 0 msg.data[1] = 2 # 方式: 2:相対位置決め(指令位置基準) msg.data[2] = 200 # 位置: 200[step] msg.data[3] = 2000 # 速度: 2000[Hz] msg.data[4] = 2000 # 起動・変速レート: 2.0[kHz/s] msg.data[5] = 2000 # 停止レート: 2.0[kHz/s] msg.data[6] = 1000 # 運転電流: 100[%] msg.data[7] = 1 # 反映トリガ: 1(全データ反映) pub.publish(msg) # クエリ生成ノードに上記内容を送信。ノードでmsg作成後はドライバに送信 wait() time.sleep(0.5) msg.slave_id = 0x01 # 号機選択(Hex): 1 msg.func_code = 0 # ファンクションコード選択: 0(Read) msg.read_addr = 204 # 先頭アドレス選択(Dec): 検出位置(step) msg.read_num = 1 # 読み込みデータサイズ: 1 (32bit) pub.publish(msg) # クエリ生成ノードに上記内容を送信。ノードでmsg作成後はドライバに送信 wait() msg.slave_id = 0x02 # 号機選択(Hex): 2 msg.func_code = 0 # ファンクションコード選択: 0(Read) msg.read_addr = 204 # 先頭アドレス選択(Dec): 検出位置(step) msg.read_num = 1 # 読み込みデータサイズ: 1 (32bit) pub.publish(msg) # クエリ生成ノードに上記内容を送信。ノードでmsg作成後はドライバに送信 wait() r.sleep() print( "FeedbackPosition1 = {0:}[step]".format(gMotor_pos1)) # 軸1の検出位置の表示 print( "FeedbackPosition2 = {0:}[step]".format(gMotor_pos2)) # 軸2の検出位置の表示 stop(msg, pub) # 運転停止 print("END") # 終了表示 rospy.spin() # msg取得用関数の実行(必須)
def main(): """メイン関数 処理内容1:運転指令(軸1 FWD方向、軸2 REV方向) 処理内容2:運転データNo.2の回転速度の変更 処理内容3:検出速度の確認 処理内容4:停止指令(軸1 減速停止、軸2 瞬時停止) """ global gMotor_spd1 global gMotor_spd2 rospy.init_node("sample3", anonymous=True) pub = rospy.Publisher("om_query1", om_query, queue_size=1) # OMにノードに送信するまでの定義 rospy.Subscriber("om_state1", om_state, stateCallback) # レスポンスのコールバック定義 rospy.Subscriber("om_response1", om_response, resCallback) # レスポンスのコールバック定義 msg = om_query() # OMノードで作成したメッセージを使用 time.sleep(1) # 1秒待機 init(msg, pub) # 初期化関数のコール # 運転指令(FWD方向)(M1,START/STOP,RUN/BRAKEをON) msg.slave_id = 0x01 # 号機選択(Hex): 1号機 msg.func_code = 1 # ファンクションコード選択: 1(Write) msg.write_addr = 124 # 先頭アドレス選択(Dec): 動作コマンド msg.write_num = 1 # 書き込みデータサイズ: 1 (32bit) msg.data[0] = 26 # 書き込みデータ: ONビット(0000 0000 0001 1010) = 26 pub.publish(msg) # クエリ生成ノードに上記内容を送信。ノードでmsg作成後はドライバに送信 wait() # 処理待ち # 運転指令(REV方向)(M1,START/STOP,RUN/BRAKE,FWD/REVをON) msg.slave_id = 0x02 # 号機選択(Hex): 2号機 msg.func_code = 1 # ファンクションコード選択: 1(Write) msg.write_addr = 124 # 先頭アドレス選択(Dec): 動作コマンド msg.write_num = 1 # 書き込みデータサイズ: 1 (32bit) msg.data[0] = 58 # 書き込みデータ: ONビット(0000 0000 0011 1010) = 58 pub.publish(msg) # クエリ生成ノードに上記内容を送信。ノードでmsg作成後はドライバに送信 wait() # 処理待ち for i in range(5): # LOOP処理(5回) # 回転速度の設定 msg.slave_id = 0x01 # 号機選択(Hex): 1号機 msg.func_code = 1 # ファンクションコード選択: 1(Write) msg.write_addr = 1156 # 先頭アドレス選択(Dec): データNo.2 回転速度 msg.write_num = 1 # 書き込みデータサイズ: 1 (32bit) msg.data[0] = 500 + i * 200 # 書き込みデータ: 500/700/900/1100/1300[r/min] pub.publish(msg) # クエリ生成ノードに上記内容を送信。ノードでmsg作成後はドライバに送信 wait() # 処理待ち # 回転速度の設定 msg.slave_id = 0x02 # 号機選択(Hex): 2号機 msg.func_code = 1 # ファンクションコード選択: 1(Write) msg.write_addr = 1156 # 先頭アドレス選択(Dec): データNo.2 回転速度 msg.write_num = 1 # 書き込みデータサイズ: 1 (32bit) msg.data[0] = 500 + i * 200 # 書き込みデータ: 500/700/900/1100/1300[r/min] pub.publish(msg) # クエリ生成ノードに上記内容を送信。ノードでmsg作成後はドライバに送信 wait() # 処理待ち time.sleep(2) # 回転速度読み込み msg.slave_id = 0x01 # 号機選択(Hex): 1号機 msg.func_code = 0 # ファンクションコード選択: 0(Read) msg.read_addr = 206 # 先頭アドレス選択(Dec): フィードバック速度[r/min](符号付) msg.read_num = 1 # 読み込みデータサイズ: 1 (32bit) pub.publish(msg) # クエリ生成ノードに上記内容を送信。ノードでmsg作成後はドライバに送信 wait() msg.slave_id = 0x02 # 号機選択(Hex): 2号機 msg.func_code = 0 # ファンクションコード選択: 0(Read) msg.read_addr = 206 # 先頭アドレス選択(Dec): フィードバック速度[r/min](符号付) msg.read_num = 1 # 読み込みデータサイズ: 1 (32bit) pub.publish(msg) # クエリ生成ノードに上記内容を送信。ノードでmsg作成後はドライバに送信 wait() print("FeedbackSpeed1 = {0:}[r/min]".format( gMotor_spd1)) # 取得した速度の表示[r/min] print("FeedbackSpeed2 = {0:}[r/min]".format( gMotor_spd2)) # 取得した速度の表示[r/min] # 減速停止 msg.slave_id = 0x01 # 号機選択(Hex): 1号機 msg.func_code = 1 # ファンクションコード選択: 1(Write) msg.write_addr = 124 # 先頭アドレス選択(Dec): 動作コマンド msg.write_num = 1 # 書き込みデータサイズ: 1 (32bit) msg.data[0] = 18 # 書き込みデータ: ONビット(0000 0000 0001 0010) = 18 pub.publish(msg) # クエリ生成ノードに上記内容を送信。ノードでmsg作成後はドライバに送信 wait() # 処理待ち # 即時停止 msg.slave_id = 0x02 # 号機選択(Hex): 2号機 msg.func_code = 1 # ファンクションコード選択: 1(Write) msg.write_addr = 124 # 先頭アドレス選択(Dec): 動作コマンド msg.write_num = 1 # 書き込みデータサイズ: 1 (32bit) msg.data[0] = 10 # 書き込みデータ: ONビット(0000 0000 0010 1010) = 10 pub.publish(msg) # クエリ生成ノードに上記内容を送信。ノードでmsg作成後はドライバに送信 wait() # 処理待ち print("END") # 終了表示 rospy.spin() # msg取得用関数の実行(必須)