紅外線感測 走線

'InnobotLineTracker.inb: Innobot Follow The Black Line
#DEFINE R_NORMAL_SPEED 50     'Set Innobot Normal Speed
#DEFINE L_NORMAL_SPEED 40
#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
Sub Main()
Dim I,Sensor As Byte
Dim R, L, Err As Integer    
Low 10: Low 11

DO
Sensor=(In(7)<<2)+(In(8)<<1)+In(9) 'Combine L,M,R Sensor State to Sensor Variable
Sensor=(~Sensor) AND &B00000111
Select Sensor 'Select Modified Parameter By 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<1 Then
    Err = ERROR_7
Elseif Err>1 Then
    Err = ERROR_1
End If
Case &B111
            Err = ERROR_4
Pause(2000) 'Wait 2 Sec.
            For I=1 To 10 'Go Forward
    Pulseout(10,300-L_NORMAL_SPEED)
    Pulseout(11,300+R_NORMAL_SPEED)
    PAUSE(16)
    Next I
         End Select
R = (300+R_NORMAL_SPEED) + Err 'Set Right Servo Parameter
        L = (300-L_NORMAL_SPEED) + Err 'Set Left Servo Parameter
    Pulseout(10,L): Pulseout(11,R): PAUSE(16) 'Output control Signal
Loop
End Sub