(7) 整合應用範例

實驗目的:
 
    將伺服機控制、 紅外線循跡、超音波避障、電子羅盤定義方向、
    重力加速度感測、藍牙傳輸…等應用整合。暸解相互的關係與應
    用方式。

 
 程式說明:
     

'模組名稱、代碼宣告

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

Peripheral mySer As ServoRunner8 @ 0

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

Peripheral mySon As SonarA @ 1 

' 設定加速度模組參數名稱 

Peripheral myG   As Accelerometer3A @ 2

' 設定電子羅盤模組參數名稱 

Peripheral myCps As CompassA @ 3 

'==========================================

'  固定常數宣告

'==========================================

'伺服機相關常數

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

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

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

'避障相關常數

#DEFINE RotateTime_L   750 '設定左旋參數

#DEFINE TSpeed_R      150 '設定右前彎,右輪速度參數

#DEFINE TSpeed_L      800 設定右前彎,左輪速度參數

'循跡相關常數

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

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

'Gsensor相關常數

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

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

'超音波相關常數

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

'電子羅盤相關常數

#DEFINE AngleLimit 10  定義偏向角極限值 

'藍牙相關常數

#DEFINE Baudmode 4044  '定義傳輸模式設定

'Baudrate = 38400

'Baudmode = (4096-(2000000\Baudrate)) 

#DEFINE RxPin 0  '定義藍牙接收腳位為 Pin 0

#DEFINE TxPin 1  '定義藍牙發射腳位為 Pin 1

'=============================================

'  全堿變數宣告

'=============================================

'紅外線相關變數

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

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

'返回路徑相關變數

Dim bFindFlag As Byte  '儲存返回路徑旗標

'伺服機相關變數

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

'Gsensor相關變數

Dim iXFource As Integer  '儲存X軸受力的相對電壓值

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

Dim bGSensorFlag As Byte '儲存感測旗標

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

'超音波相關變數

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

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

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

'電子羅盤相關變數

Dim bCalFlag As Byte  '儲存羅盤校正完成旗標

Dim bDirFlag As Byte  '儲存方向校正完成旗標

Dim wAngle As Word   '儲存角度值

Dim iAngle As Integer     儲存偏向角度

'藍牙相關變數

Dim bGetData As Byte  '儲存接收資料

Dim bSendData As Byte  '儲存傳輸資料

Dim sSendData As String*20  儲存傳輸資料

'===========================================

  主程式區塊

'===========================================

Sub Main()

  '初始設定

  StartSet()

  Pause 5000

 

  '確認前進方向

  Do

      DirReturn(0) 

  Loop Until bDirFlag = 1

 

  '尋找路徑

  Forward()

  Do

      IRScan()

      If bSenSor <> 111 Then

          FindLine()

      End If

  Loop Until bFindFlag = 1

 

  '偵測障礙物

  Do

      IRScan()

      Adjustment()

      Forward()

  Loop Until bDisFlag = 1

 

  '避障,尋找路徑

  Avoid()

  bFindFlag = 0

  Do

      IRScan()

      If bSenSor <> 111 Then

          FindLine()

      End If

  Loop Until bFindFlag = 1

 

  '偵測斜坡 

  iSpeed = 0

  Do

      IRScan()

      Adjustment()

      Forward()

      '偵測是否發現斜坡

      ScanSlope()

  Loop Until bGsensorFlag = 1

 

'條件迴圈,前進直到發現平地

  Do

      IRScan()

      Adjustment()

      Forward()

      '偵測是否發現平地

      ScanGND()

  Loop Until bGsensorFlag = 1 

 

  Stop()

  '發送完成任務字串

  sSendData = "Innovati"

  SendString() 

End Sub

'============================================

  副程式區塊

'===========================================

'初始設定

Sub StartSet()

  TrackSet()  '循跡相關設定

  CompassSet() 電子羅盤設定

  SonarSet()  '超音波相關設定

  GsensorSet()  'Gsensor相關設定

  FindLineSet()  '避障相關設定

End Sub

 

'=============================================

  伺服機相關副程式

'=============================================

Sub FindLineSet()  ' 尋找路徑相關設定

  bFindFlag = 0

  iSpeed = 0

End Sub

 

Sub Stop()  停止

    mySer.SetPosAndRun(7,1500)

    mySer.SetPosAndRun(6,1500)

End Sub

 

Sub Forward()

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

   mySer.SetPosAndRun(6,1500 - NormalSpeed_R + iSpeed)

End Sub

 

Sub Backward()  後退

    mySer.SetPosAndRun(7,1500 - NormalSpeed_L)

    mySer.SetPosAndRun(6,1500 + NormalSpeed_R) 

End Sub

 

Sub Rotate_R()  右旋

    mySer.SetPosAndRun(7,1500 + NormalSpeed_L)

    mySer.SetPosAndRun(6,1500 + NormalSpeed_R) 

End Sub

 

Sub Rotate_L()  '左旋

    mySer.SetPosAndRun(7,1500 - NormalSpeed_L)

    mySer.SetPosAndRun(6,1500 - NormalSpeed_R) 

End Sub

 

Sub Turn_R()   右轉

    mySer.SetPosAndRun(7,1500 + NormalSpeed_L)

    mySer.SetPosAndRun(6,1500 ) 

End Sub

 

Sub Turn_L()  左轉

    mySer.SetPosAndRun(7,1500 )

    mySer.SetPosAndRun(6,1500NormalSpeed_R) 

End Sub

 

Sub Avoid()

    避開障礙物

    Rotate_L()

    '執行左旋時間長度為RotateTime_L

    Pause RotateTime_L

    mySer.SetPosAndRun(7,1500 + TSpeed_L)

    mySer.SetPosAndRun(6,1500 - TSpeed_R)

    Pause 300

End Sub

 

Sub FindLine() 

    Do

        IRScan()

    Loop Until bSenSor = 111

    Pause 250

   

    Rotate_L()

    Do

        IRScan()

    Loop Until bIR10 = 0

    Stop()

    Pause 200

    bFindFlag = 1

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   

 

 

'===============================================

  Gsensor相關副程式

'===============================================

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

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

End Sub

'量測X軸受力值,是否為斜坡

Sub ScanSlope()

    myG.GetXForce(iXFource)

    bGSensorFlag = 0

    wGCnt = 0

    判斷是否為斜坡,若是則連續偵測。

    當連續偵測為斜坡的次數達到設定次數,則確認為斜坡。

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

       myG.GetXForce(iXFource)  wGCnt = wGCnt + 1 

     Loop

   

    If wGCnt > CheckCnt Then

         bGSensorFlag = 1

    End If

End Sub

'量測X軸受力值,是否為平地

Sub ScanGND()

    myG.GetXForce(iXFource)

    bGSensorFlag = 0

    wGCnt = 0

 判斷是否為平地,若是則連續偵測。

 當連續偵測為平地的次數達到設定次數,則確認為平地。       

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

      myG.GetXForce(iXFource)  wGCnt = wGCnt + 1 

   Loop

  If wGCnt > CheckCnt Then

      bGSensorFlag = 1

  End If

 

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

    bCalFlag = 0

    bDirFlag = 0

    myCps.Calibration(0)  啟動校正模式

    Do:Loop Until bCalFlag = 1 條件迴圈,直到校正完成

    Pause 120

    DirSet()

End Sub

 

 

'定義前、後、左、右 對應的角度數值

Sub DirSet()

    區堿變數宣告

    Dim I As Byte    迴圈變數

    myCps.GetAngle(wAngle) 取得當前角度 

    myCps.SaveAngle(0,wAngle)

    For I = 1 To 3

        wAngle = wAngle + 90

        If wAngle > 360 Then

            wAngle = wAngle -360

        End If

        myCps.SaveAngle(I,wAngle)

    Next

End Sub

 

Event myCps.CalEndEvent()

  bCalFlag = 1

End Event

 

'bDir所指定的方向,做為校正方向

Sub DirReturn(bDir As Byte)  可附加參數副程式

    '取得與設定為基準方位差異的偏向角

      myCps.GetDevAngle(bDir,iAngle) 

      If iAngle > AngleLimit Then

          Rotate_R() 

      Elseif iAngle < -AngleLimit Then

          Rotate_L()

      Else

          bDirFlag = 1 

      End If

End Sub

 

'===============================================

  藍牙相關副程式

'===============================================

Sub GetByte()

   接收資料  Serin RxPin,Baudmode,[bGetData] 

End Sub

 

Sub SendByte(bSend As Byte)  '可附加參數副程式

  '傳送資料

  Serout TxPin,Baudmode,[bSend] 

End Sub

 

Sub SendString()

  '傳送資料  

   Serout TxPin,Baudmode,[sSendData] 

End Sub

 
Comments