自走車避障

'InnobotLineTracker.inb: Innobot Follow The Black Line
#DEFINE NORMAL_SPEED 40     'Set Innobot Normal Speed
#DEFINE ERROR_1     60 'Define Error for each Sensor Value
#DEFINE ERROR_2     30
#DEFINE ERROR_3     10
#DEFINE ERROR_4     0
#DEFINE ERROR_5   -10
#DEFINE ERROR_6   -30
#DEFINE ERROR_7   -60
Peripheral MySonar As SonarA @ 3
Sub Main()
Dim Distance As Word =0
Dim I,J,K,H,A,Sensor As Byte
Dim R, L,M,N,d, Err  As Integer
Dim B  As Integer =0    
Low 10: Low 11
DO
MySonar.Ranging()
MySonar.GetDistance(1,Distance)
Sensor=(In(7)<<2)+(In(8)<<1)+In(9)
If (Distance>25 Or Distance=0) Then '超音波偵測距離大於25或等於0時執行
Select Sensor
Case &B100
Err = ERROR_2
Case &B110
Err = ERROR_3
      Case &B010
Err = ERROR_4
        Case &B011
Err = ERROR_5
        Case &B001
Err = ERROR_6
        Case &B000
If Err<0 Then
    Err = ERROR_7
Elseif Err>0 Then
    Err = ERROR_1
End If
Case &B111
            Err = ERROR_4
Pause(200)
n+=1 '從開始時路徑上偵測到次交叉點n就+1
If (m=1) Then '當m=1時執行蜂鳴器程序
For d=0 To n Step d+1 '從0迴圈到n次，n為偵測交叉點次數
Sound(12,500,768)
   Next d
Pause(20000) '迴轉180度之後遇交叉點停下
End If
         End Select
R = (300+NORMAL_SPEED) + Err
        L = (300-NORMAL_SPEED) + Err
    Pulseout(10,L): Pulseout(11,R): PAUSE(16)
Else '超音波偵測小於25時開始轉彎繞行
If(b=0) Then '變數b=0時執行此段繞行障礙
For J=1 TO 20 '左轉45度
Pulseout(10,230)
Pulseout(11,260)
Pause(20)
Next J

FOR H=1 TO 70 '左轉後直行
Pulseout(10,250)
Pulseout(11,350)
Pause(20)
Next H

For K=1 TO 58 '右轉90度
Pulseout(10,320)
Pulseout(11,360)
Pause(20)
Next K

FOR A=1 TO 70 '右轉後直行
Pulseout(10,265)
Pulseout(11,335)
Pause(20)
Next A
b=1 '執行過一次繞行後b=1,以便於第2次遇障礙物執行迴轉180度
Else
For K=1 TO 76 '第2次遇障礙物時迴轉180度
Pulseout(10,250)
Pulseout(11,250)
Pause(20)
Next K
m=1 '偵測到第2次而迴轉180度之後設定m=1
 End If
End If
Loop
End Sub