'{$STAMP BS2sx}

'{$PBASIC 2.5}

SO CON 14

BUSY VAR IN15

BAUD CON 16624

L_DIFFUSE CON 0

R_DIFFUSE CON 13

L_VALUE VAR WORD

R_VALUE VAR WORD

DEVIATION CON 500

DECISION CON 220

ERROR VAR WORD

I VAR BYTE

L VAR BYTE

R VAR BYTE

PAUSE 1000

SEROUT SO,BAUD,[128,"M",4,128,"S",40]

GOSUB WAIT_BUSY

GOSUB CALIBRATE

MAIN: HIGH L_DIFFUSE

PAUSE 1

RCTIME L_DIFFUSE,1,L_VALUE

HIGH R_DIFFUSE

PAUSE 1

RCTIME R_DIFFUSE,1,R_VALUE

L_VALUE=L_VALUE-ERROR

IF L_VALUE<DECISION THEN REDIRECT_R

IF R_VALUE<DECISION THEN REDIRECT_L

L=4:R=4

GOSUB DRV

GOTO MAIN

REDIRECT_L: L=0:R=1

GOSUB DRV

GOTO MAIN

REDIRECT_R: L=1:R=0

GOSUB DRV

GOTO MAIN

DRV: SEROUT SO,BAUD,[L,R]

WAIT_BUSY: IF BUSY=1 THEN WAIT_BUSY

RETURN

CALIBRATE: HIGH L_DIFFUSE

PAUSE 1

RCTIME L_DIFFUSE,1,L_VALUE

HIGH R_DIFFUSE

PAUSE 1

RCTIME R_DIFFUSE,1,R_VALUE

ERROR= L_VALUE-R_VALUE

RETURN

//*****************************************************

 //       หลักการทำงานของโปรแกรมหุ่นยนต์เดินตามเส้น

//*****************************************************

           โปรแกรมหุ่นยนต์เดินตามเส้นจะมีอุปกรณ์ที่สำคัญคือ diffuse

     หลักการทำงานของ diffuse คือการส่องแสงสีแดงออกไปแล้วรอให้แสงสะท้อนกลับมา ค่าที่แสงสะท้อนกลับมานั้นสีขาว กับสีดำจะไม่เท่ากัน เพราะแต่ละสีมีการดูดซับและสะท้อนแสงไม่เท่ากัน สีขาวจะมีค่าทอ่านได้ ต่ำกว่า สีดำ

    เราจะสั่งให้เดินตามเส้นสีขาวพื้นสีดำ (โดยติดตัว diffuse คร่่อมเส้น) ก็คือถ้าค่าที่สะท้อนกลับมามีค่าน้อยกว่าค่า dicition ที่กำหนดไว้ ให้หุ่นยนต์เลี้ยวหาเส้น กล่าวคือถ้า diffuse ด้านขวารับค่าแสงสะท้อนมาน้อยกว่าค่าที่กำหนด ให้หุ่นยนต์เลี้ยวซ้ายเพื่อกลับเข้าหาเส้น ในทางกลับกัน ถ้าdiffuse ด้านซ้ายรับค่าแสงสะท้อนมาน้อยกว่าค่าที่กำหนด ให้หุ่นยนต์เลี้ยวขวาเพื่อกลับเข้าหาเส้น