โปรเเกรมควบคุมหุ่นยนต์ให้เดินตามเส้นพร้อมหลบหลีกสิ่งกีดขวาง

' {$STAMP BS2p}
loopLeft   VAR  Word
loopright  VAR  Word
loopStraight  VAR  Word

loopMain:
IF(IN4=0 AND IN3=0 AND IN1=0 AND IN0=0)THEN motorstop
IF(IN4=0 AND IN3=0 AND IN1=0 AND IN0=1)THEN motorstop
IF(IN4=0 AND IN3=0 AND IN1=1 AND IN0=0)THEN motorstop
IF(IN4=0 AND IN3=0 AND IN1=1 AND IN0=1)THEN motorbackward
IF(IN4=0 AND IN3=1 AND IN1=0 AND IN0=0)THEN motorstop
IF(IN4=0 AND IN3=1 AND IN1=0 AND IN0=1)THEN motorstop
IF(IN4=0 AND IN3=1 AND IN1=1 AND IN0=0)THEN motorstop
IF(IN4=0 AND IN3=1 AND IN1=1 AND IN0=1)THEN motorevadeLeft
IF(IN4=1 AND IN3=0 AND IN1=0 AND IN0=0)THEN motorstop
IF(IN4=1 AND IN3=0 AND IN1=0 AND IN0=1)THEN motorstop
IF(IN4=1 AND IN3=0 AND IN1=1 AND IN0=0)THEN motorstop
IF(IN4=1 AND IN3=0 AND IN1=1 AND IN0=1)THEN motorEvadeRight
IF(IN4=1 AND IN3=1 AND IN1=0 AND IN0=0)THEN motorForward
IF(IN4=1 AND IN3=1 AND IN1=0 AND IN0=1)THEN motorForward
IF(IN4=1 AND IN3=1 AND IN1=1 AND IN0=0)THEN motorForward
IF(IN7=0 AND IN6=0 AND IN5=0 AND IN4=1 AND IN3=1 AND IN1=1 AND IN0=1)THEN motorForward
IF(IN7=0 AND IN6=0 AND IN5=1 AND IN4=1 AND IN3=1 AND IN1=1 AND IN0=1)THEN motorReturnRightGoLine
IF(IN7=0 AND IN6=1 AND IN5=0 AND IN4=1 AND IN3=1 AND IN1=1 AND IN0=1)THEN motorForward
IF(IN7=0 AND IN6=1 AND IN5=1 AND IN4=1 AND IN3=1 AND IN1=1 AND IN0=1)THEN motorReturnright90
IF(IN7=1 AND IN6=0 AND IN5=0 AND IN4=1 AND IN3=1 AND IN1=1 AND IN0=1)THEN motorReturnLeftGoLine
IF(IN7=1 AND IN6=0 AND IN5=1 AND IN4=1 AND IN3=1 AND IN1=1 AND IN0=1)THEN motorForward
IF(IN7=1 AND IN6=1 AND IN5=0 AND IN4=1 AND IN3=1 AND IN1=1 AND IN0=1)THEN motorReturnLeft90
IF(IN7=1 AND IN6=1 AND IN5=1 AND IN4=1 AND IN3=1 AND IN1=1 AND IN0=1)THEN motorForward
GOTO loopmain
'##########motorforward##########
motorforward:
PAUSE 10
IF(IN4=1 AND IN3=1 AND IN1=0 AND IN0=0)THEN nextmotorforward
IF(IN4=1 AND IN3=1 AND IN1=0 AND IN0=1)THEN nextmotorforward
IF(IN4=1 AND IN3=1 AND IN1=1 AND IN0=0)THEN nextmotorforward
IF(IN4=1 AND IN3=1 AND IN1=1 AND IN0=1)THEN nextmotorforward
IF(IN7=0 AND IN6=0 AND IN5=0 AND IN4=1 AND IN3=1 AND IN1=1 AND IN0=1)THEN nextmotorforward
IF(IN7=0 AND IN6=1 AND IN5=0 AND IN4=1 AND IN3=1 AND IN1=1 AND IN0=1)THEN nextmotorforward
IF(IN7=1 AND IN6=0 AND IN5=1 AND IN4=1 AND IN3=1 AND IN1=1 AND IN0=1)THEN nextmotorforward
GOTO loopmain
nextmotorforward:
     PULSOUT 13,1250
     PULSOUT 14,2500
     PAUSE 20
GOTO loopmain
'###########motorBackward#########
motorBackward:
  PAUSE 10
IF(IN4=0 AND IN3=0 AND IN1=1 AND IN0=1)THEN Nextmotorbackward
  GOTO loopmain
nextmotorbackward:
  FOR loopstraight=1 TO 50
    PULSOUT 12,2500
    PULSOUT 14,1250
    PAUSE 20
  NEXT
GOTO loopmain
'##########motorstop##########
motorstop:
PAUSE 10

           IF(IN4=0 AND IN3=0 AND IN1=0 AND IN0=0)THEN nextmotorstop
           IF(IN4=0 AND IN3=0 AND IN1=0 AND IN0=1)THEN nextmotorstop
           IF(IN4=0 AND IN3=0 AND IN1=1 AND IN0=0)THEN nextmotorstop
           IF(IN4=0 AND IN3=1 AND IN1=0 AND IN0=0)THEN nextmotorstop
           IF(IN4=0 AND IN3=1 AND IN1=0 AND IN0=1)THEN nextmotorstop
           IF(IN4=0 AND IN3=1 AND IN1=1 AND IN0=0)THEN nextmotorstop
           IF(IN4=1 AND IN3=0 AND IN1=0 AND IN0=0)THEN nextmotorstop
           IF(IN4=1 AND IN3=0 AND IN1=0 AND IN0=1)THEN nextmotorstop
           IF(IN4=1 AND IN3=0 AND IN1=1 AND IN0=0)THEN nextmotorstop
           GOTO loopmain
nextmotorstop:
      LOW 13
      LOW 14
      FREQOUT 15,1000,1000,1000
      GOTO loopmain
'#########motorReturnLeft90###########
motorReturnLeft90:
           PAUSE 10
 IF(IN7=1 AND IN6=1 AND IN5=0 AND IN4=1 AND IN3=1 AND IN1=1 AND IN0=1)THEN nextmotorReturnLeft90
 IF(IN7=1 AND IN6=1 AND IN5=1 AND IN4=1 AND IN3=1 AND IN1=1 AND IN0=1)THEN nextmotorReturnLeft90
  GOTO loopmain
nextmotorreturnleft90:
 FOR loopleft=1 TO 30
 PULSOUT 13,1250
 PULSOUT 14,2500
 PAUSE 20
NEXT
FOR loopleft=1 TO 40
  PULSOUT 13,1250
  PULSOUT 14,1250
 PAUSE 20
NEXT
GOTO loopmain
'##########motorevaderight##########
motorevaderight:
PAUSE 10
IF(IN4=1 AND IN3=0 AND IN1=1 AND IN0=1)THEN nextmotorevaderight
GOTO loopmain
nextmotorevaderight:
FOR loopright=1 TO 40
PULSOUT 13,2500
PULSOUT 14,1250
PAUSE 20
NEXT
FOR loopright=1 TO 20
PULSOUT 13,1250
PULSOUT 14,1250
PAUSE 20
NEXT
GOTO loopmain
'##########motorreturnleftgoline##########
motorreturnleftgoline:
PAUSE 10
IF(IN7=1 AND IN6=0 AND IN5=0 AND IN4=1 AND IN3=1 AND IN1=1 AND IN0=1)THEN nextmotorreturnleftgoline
GOTO loopmain
nextmotorreturnleftgoline:
PULSOUT 13,1250
PULSOUT 14,1250
PAUSE 20
GOTO loopmain
'##########motorReturnRight90##########
motorreturnright90:
PAUSE 10
IF(IN7=0 AND IN6=1 AND IN5=1 AND IN4=1 AND IN3=1 AND IN1=1 AND IN0=1)THEN NEXTmotorreturnright90
GOTO loopmain
nextmotorreturnright90:
FOR loopright=1 TO 30
PULSOUT 13,1250
PULSOUT 14,2500
PAUSE 20
NEXT
FOR loopright= 1 TO 40
PULSOUT 13,2500
PULSOUT 14,2500
PAUSE 20
NEXT
GOTO loopmain
'##########motorevadeleft##########
motorevadeleft:
PAUSE 10
IF(IN4=0 AND IN3=1 AND IN1=1 AND IN0=1)THEN NEXTmotorevadeleft
GOTO loopmain
NEXTmotorevadeleft:
FOR loopleft=1 TO 40
PULSOUT 13,2500
PULSOUT 14,1250
PAUSE 20
NEXT
FOR loopleft=1 TO 20
PULSOUT 13,2500
PULSOUT 14,2500
PAUSE 20
NEXT
GOTO loopmain
'##########motorreturnrightgoline##########
motorreturnrightgoline:
PAUSE 10
IF(IN7=0 AND IN6=0 AND IN5=1 AND IN4=1 AND IN3=1 AND IN1=1 AND IN0=1)THEN NEXTmotorreturnrightgoline
GOTO loopmain
NEXTmotorreturnrightgoline
PULSOUT 13,2500
PULSOUT 14,2500
PAUSE 20

GOTO loopmain