練習 2 :紅外線循跡

    經過練習 1 的實驗,我們可以暸解紅外線訊號與顏色的關係。因此我們可以透過程式將紅外線偵測的結果用於以黑底白線為基礎的路線來達成循跡的用途。
 
 程式說明: 

'模組名稱、代碼宣告

Peripheral mySer As ServoRunner8 @ 0

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

'固定常數宣告

#DEFINE NormalSpeed_L 300  設定左輪伺服機速度預設值

#DEFINE NormalSpeed_R 300  設定右輪伺服機速度預設值

#DEFINE iSpeedMAX  300     '設定校正速度的最大值

#DEFINE iErr1  5              '設定錯誤修正參數1

#DEFINE iErr2 45             '設定錯誤修正參數2

'全堿變數宣告

'紅外線相關變數

Dim bIR8,bIR9,bIR10 As Byte  ' 儲存紅外線感測值

Dim bSensor As Byte         ' 儲存紅外線感測值總合

'伺服機相關變數

Dim iSpeed As Integer        ' 儲存伺服機速度校正值

Sub Main()  '主程式區塊

  TrackSet() '初始設定 

  '無窮回圈,反覆執行紅外線感測與路線校正

  Do

        IRScan()      取得紅外線資訊

        Adjustment()  校正

        Forward()     執行前進

  Loop

 

End Sub

 

Sub TrackSet()  ' 循跡相關設定

  bIR8 = 0

  bIR9 = 0

  bIR10 = 0

  iSpeed = 0

  bSensor = 0

End Sub

Sub IRScan()  ' 讀取所有紅外線感測值

    bIR8 = In(8) ' 讀取紅外線感測器回傳值,存放於bIR8中。

    bIR9 = In(9)' 讀取紅外線感測器回傳值,存放於bIR9中。

    bIR10 = In(10) ' 讀取紅外線感測器回傳值,存放於bIR10中。
    '將偵測到的值拼為一個判斷值
    bSensor = (100 * bIR8) + (10 * bIR9) + bIR10 
End Sub
 

Sub Adjustment()  ' 依照Sensor的參數內容,校正速度。

    '根據感測器回傳值,判斷軌道位置並設定修改速度值。

    Select Case bSenSor

           Case 110 : iSpeed = iSpeed + iErr2  '黑黑白

           Case 100 : iSpeed = iSpeed + iErr1  '黑白白

           Case 001 : iSpeed = iSpeed - iErr1  '白白黑

           Case 011 : iSpeed = iSpeed - iErr2  '白黑黑     

           Case Else : iSpeed = 0

    End Select   

   

    '確認修正參數是否超過極限值

    If iSpeed > iSpeedMAX Then

      iSpeed = iSpeedMAX

    Elseif iSpeed < -iSpeedMAX Then

      iSpeed = -iSpeedMAX

    End If

 

End Sub   

 

'前進副程式,由初始定義的速度加上修正的速度參數達成控制

Sub Forward()

  mySer.SetPosAndRun(7,1500 + NormalSpeed_L + iSpeed)

  mySer.SetPosAndRun(6,1500NormalSpeed_R + iSpeed) 

End Sub

 

 

 
 補充說明:
 
   (1)iErr1、iErr2可依路徑需求與行進的速度修正對應的修改參數。
 
 
   (2) Adjustment()副程式中的內容是依照當時的紅外線感測值調整速
        度的修正值。            
   (3)定義修正值的極限是必免校正過後的速度值會超出伺服機的控制
        範圍。∴ 初始速度  +  校正速度 <= 1000
 
Comments