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