練習 2:偵測傾斜度


實驗目的:
 
   判斷X軸的受力變化,檢查是否抵達斜坡。若抵達斜坡則停止前進。

 
 程式說明:
 
' 設定加速度模組參數名稱

Peripheral myG As Accelerometer3A @ 2

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

Peripheral mySer As ServoRunner8 @ 0

'固定常數宣告

'伺服機相關常數

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

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

'Gsensor相關常數

#DEFINE CheckCnt    100  '定義偵測確認計數器數值範圍

#DEFINE GsenserLimit  50  '定義受力極限值範圍

'全堿變數宣告

'Gsensor相關變數

Dim iNormalX As Integer '儲存X軸初始值

Dim iXFource As Integer '儲存X各軸向受力的數位化電壓值

Dim bGSensorFlag As Byte 儲存感測旗標

Dim wGCnt As Word   儲存極限值提醒次數

 

Sub Main()  主程式區塊

      GsensorSet() 加速度模組相關設定    

      Forward()   

      條件迴圈,若wGCnt > CheckCnt 則離開回圈

      Do 

      GsensorScan()

      LOOP Until bGsensorFlag = 1   

    Stop()

End Sub
 

Sub GsensorSet() 加速度模組相關設定

       myG.GetXForce(iNormalX) 取得當前X軸受力值

End Sub

 

'量測X軸受力值,若超出設定範圍視為斜坡

Sub GsensorScan()

    myG.GetXForce(iXFource)

    bGSensorFlag = 0

    wGCnt = 0

X軸受力值,超出設定範圍時,進入條件迴圈。

迴圈條件為X軸受力值超出設定範圍且計數認數未達到設定次數。

    Do While iXFourc < (iNormalX-GsenserLimit) And wGCnt <= CheckCnt 

          myG.GetXForce(iXForce)

         wGCnt = wGCnt + 1

    Loop  

 

  若量測結果達到設角的連續次數,則啟動旗標。

  If wGcnt > CheckCnt Then

     bGSensorFlag = 1

  End IF

End Sub

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

 

 
補充說明:
 
    (1)選擇使用X軸的受力方式來判斷是由於安裝方式,並非特定
 
    (2)GsensorSet()副程式中,取得X軸當前受力值為平地時的受力
        參考值,因此程式啟動時需將機器人置於平地必免參考值有誤。
 
    (3)由於瞬間加速度會影響受力值,因此透過下列程式以Do While
         的方式,當受力值達到設定範圍計數次數未達標準時返覆
        偵測受力值若受力值連續達到設定範圍則確定為發現斜坡。
 

Do While iXFource < (iNormalX-GsenserLimit) And wGCnt <= CheckCnt                                          

     myG.GetXForce(iXForce)

     wGCnt = wGCnt + 1

Loop
 
Comments