練習 3:自動校正方位


實驗目的:
 
    將電子羅盤與伺服機應用結合,當偏向角度大於設定值時自動修正角度。
 
 程式說明:
  

'模組名稱、代碼宣告

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

Peripheral mySer As ServoRunner8 @ 0

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

Peripheral myCps As CompassA @ 3

'固定常數宣告

'電子羅盤相關

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

'伺服機相關

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

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

'全堿變數宣告

'電子羅盤相關變數

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

Dim wAngle As Word   '儲存角度值

Dim iAngle As Integer    儲存偏向角度

 

Sub Main()  '主程式區塊

   CompassSet() 

  '無窮迴圈,反覆取得與設定為基準方位差異的偏向角

   Do 

       DirReturn(0)  

   LOOP   

End Sub

 

Sub CompassSet()

  bCalFlag = 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)  'Forward

    For I = 1 To 3

          wAngle = wAngle + 90

          If wAngle > 360 Then

              wAngle = wAngle -360

          End If

          myCps.SaveAngle(I,wAngle)

    Next 

End Sub

 

'校正完成事件,此事件為預設開啟。無需使用者啟動

'當校成完成事,將校正旗標設為 1

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

          Stop() 

      End If

End Sub

 

Sub Stop()  '停止

    mySer.SetPosAndRun(7,1500)

    mySer.SetPosAndRun(6,1500)

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,1500NormalSpeed_L)

    mySer.SetPosAndRun(6,1500NormalSpeed_R)

End Sub

 

 
補充說明: 
 
    (1) 由於CompassA可偵測週圍環境,定義出模組與磁力最強方向
     的夾角θ

     因此於相同的環境下,可推算出前、後、左、右的對應角度。

     以下圖為例,當機器人面朝前方時測得角度θ。因此可以推算,

     當機器人面朝右方時模組會取得的角度為θ+90°依照上述方

     式即可取得其它方向的對應角度!

  
    (2) DirSet() 將前、右、後、左的相對角度存放於,篇號0~3的角度
         暫存器中。
 
    (3) DirReturn(bDir AS Byte) 此副程式較為特殊,為可附加參數副程
          式。以此程式為例當宣告DirReturn(0)時,將產生一個內容為 0
          的區堿變數bDir。
 
    (4) myCps.GetDevAngle(bDir,iAngle),將以bDir所指向的角度暫存
          器內容為參考角,並取得偏向角於iAngle。
 
    (5) 使用者可嚐試將DirRetrn的參數修改為0~3之間的數字,機器人
          將會依照偏向角的修正,轉向指定的方向。
Comments