Zenbo Junior的聲納感測器測試

機器人最重要的能力是移動,而在移動的過程中,透過感測器來偵測週遭的環境,再根據收集到的資料進行回應,是必需具備的功能之一。Zenbo Junior本身具有許多的感測器,其中聲納系統應該是比較實用的其中一個感測器,因為它可以對週遭的物體進行距離的偵測,方便我們根據物體的距離來決定是否繼續前進。

不過,Zenbo Junior利用聲納系統取得距離的方式不太直覺,以下是它的呼叫函式:

data = sdk.sensor.get_sonar(2)

看到這一行,一般的想法是在執行完畢之後,data中的內容應該是距離了吧?其實不是,它實際上傳回的內容如下:

(7, {'state': 5, 'error': 0})

它傳回的內容基本上和距離沒有關係,神奇吧!它是一個元組型態,第1個值代表指令執行的序號,第2個元素是一個字典,裡面告訴你這個指令是否執行成功,也就是執行後的狀態。如果state是5的話表示執行順利被執行,後面的error的內容就會是零,否則它會傳回錯誤代碼。

那麼,實際上的距離會被放在哪裡呢?事實上在執行完get_sonar函式之後,它會去呼叫on_result_callback所設定的回呼函式中,所以,想要得到實際偵測到的距離,要先寫一個callback函式,設定完成之後再去接收。該callback函式實際上接收到的資訊如下:

{'serial': 4, 'cmd': 655371, 'error': 0, 'result': {'SONAR_POSITION': 'FC', 'SENSOR_VALUE': 0.34599998593330383}}

有了以上的基本知識之後,我們就可以寫一個一面行去,一面偵測目前距離的程式了:

import pyzenbo, time
from pyzenbo.modules.dialog_system import RobotFace

result=dict()
def on_result(**kwargs):
    global result
    result = kwargs

host = open('ip.txt', "r", encoding="utf-8").read()
sdk = pyzenbo.connect(host)
sdk.on_result_callback = on_result
sdk.robot.set_expression(RobotFace.PLEASED, "開始前進")
sdk.motion.remote_control_body(1, True, 100)
for i in range(5):
    data = sdk.sensor.get_sonar(2,True, 10)
    try:
        dist = result['result']['SENSOR_VALUE']
        print(dist)
        sdk.robot.speak(str(round(dist*100))+"公分")
    except:
        pass
    time.sleep(2)
    
sdk.motion.remote_control_body(0, True,100)
sdk.robot.set_expression(RobotFace.PLEASED, "任務結束")
sdk.robot.set_expression(RobotFace.HIDEFACE) 
sdk.release()

發佈留言

發佈留言必須填寫的電子郵件地址不會公開。 必填欄位標示為 *