'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