練習 2 :偵測障礙物

實驗目的:
 
    將練習 1 的程式與伺服機的控制結合,偵測前方是否有障礙物當沒有障礙物時執行前進若有障礙物則停止前進。 

程式說明:
 

'設定超音波模組參數名稱

Peripheral mySon As SonarA @ 1

設定伺服機控制模組參數名稱

Peripheral mySer As ServoRunner8 @ 0 

'固定常數宣告

'伺服機相關常數

#DEFINE NormalSpeed_L 500 設定左輪伺服機速度參數

#DEFINE NormalSpeed_R 500 設定右輪伺服機速度參數

'超音波相關常數

#DEFINE StopDis 15    '設定停止安全距離

'全堿變數宣告

'超音波相關變數

Dim bSonStatus As Byte  '儲存超音波偵測狀況

Dim wDistance As Word   '儲存偵測距離值

Dim bDisFlag As Byte  '儲存距離感測旗標

Sub Main()  主程式區塊

    SonarSet() 超音波相關設定

    '無窮迴圈,反覆偵測距離執行避障動作

    Do

    '若距離感測旗標為0執行前進動作,

    反之執行右旋轉 0.5 秒避開障礙物

      If bDisFlag = 0 Then

          Forward()

      Elseif bDisFlag = 1 Then

          Stop()

      End If

    LOOP  

 

End Sub

 

 

Sub SonarSet() 超音波相關設定

        mySon.EnableRangingFinishEvent() 啟動偵測完成事件

        mySon.RepeatRanging()  啟動重復偵測

        bSonStatus = 0

End Sub

 

Event mySon.RangingFinishEvent()

   '取得偵測狀態與距離

   bSonStatus = mySon.GetDistance(1,wDistance)    

   If bSonStatus = 1 Then若偵測值為有效,即顯示於終端視窗

   '若距離結果小於停止距離則將感測旗標設為1,反之設為0

   If wDistance < StopDis Then

   bDisFlag = 1

   Else

       bDisFlag = 0  End If

   End If 

End Event

Sub Forward()   '前進

  mySer.SetPosAndRun(7,1500 + NormalSpeed_L)

  mySer.SetPosAndRun(6,1500NormalSpeed_R)

End Sub

Sub Stop()  '停止

  mySer.SetPosAndRun(7,1500 )

  mySer.SetPosAndRun(6,1500 ) 

End Sub

   

 
補充說明:
 
     #DEFINE StopDis 15 為偵測停止距離,當偵測距離小於 
    StopDis 時則判斷前方出現障礙物。
Comments