Remote Control

' {$STAMP BS2sx}
' {$PBASIC 2.5}
KEY VAR Byte
main: DO
SERIN 3,2063,250,timeout,[KEY]
IF KEY = "D" OR KEY = "d" THEN GOSUB forward
IF KEY = "A" OR KEY = "a" THEN GOSUB backwarD
IF KEY = "C" OR KEY = "c" THEN GOSUB t_left
IF KEY = "B" OR KEY = "b" THEN GOSUB t_right
LOOP
GOTO main
timeout: LOW 13 : LOW 12 : LOW 15 : LOW 14 : RETURN
forward: HIGH 13 : LOW 12 : HIGH 15 : LOW 14 : RETURN
backward: HIGH 12 : LOW 13 : HIGH 14 : LOW 15 : RETURN
S_left: HIGH 13 : LOW 12 : HIGH 14 : LOW 15 : RETURN
S_Right: HIGH 12 : LOW 13 : HIGH 15 : LOW 14 : RETURN
T_Right: LOW 13 : LOW 12 : HIGH 15 : LOW 14 : RETURN
T_LEFT: HIGH 13 : LOW 12 : LOW 15 : LOW 14 : RETURN
Motor_off:LOW 13 : LOW 12 : LOW 15 : LOW 14 : RETURN

รวม Sensor

ตรวจสอบระยะห่างด้วย ADC ว่าถ้าระยะใกล้มากกว่าตัวเลขที่กำหนดเช่น 385 โปรแกรมจะคำนวณตามสูตร
สูตรต่างกันที่ ( ADC > 385(ใกล้ที่สุด),320(3cm),250(5cm) )

วัตถุประสงค์

วิชา RobotPrograming เรียนไปเพื่อ ทำให้เขียนโปรแกรมเป็น และนำไปประยุกต์ใช้

Sensor 3cm

Code:
' {$STAMP BS2sx}
' {$PBASIC 2.5}
' {$PORT COM1}
ADC VAR Word
R VAR Word
I VAR Byte
X VAR Word
PAUSE 1000
HIGH 10
DO
GOSUB forward
GOSUB RD_ADC
IF ( ADC > 320 ) THEN
GOSUB backward : PAUSE 80
GOSUB motor_off : PAUSE 10000
ENDIF
LOOP
RD_ADC: LOW 10 : PAUSE 1 : HIGH 10
SEROUT 10,240,[1]
SERIN 10,240,[ADC.BYTE0,adc.BYTE1]
RETURN
Error: DEBUG "Error Reading" , CR
RETURN
forward: HIGH 13 : LOW 12 : HIGH 15 : LOW 14 : RETURN
backward: HIGH 12 : LOW 13 : HIGH 14 : LOW 15 : RETURN
S_left: HIGH 13 : LOW 12 : HIGH 14 : LOW 15 : RETURN
S_Right: HIGH 12 : LOW 13 : HIGH 15 : LOW 14 : RETURN
T_Right: LOW 13 : LOW 12 : HIGH 15 : LOW 14 : RETURN
T_LEFT: HIGH 13 : LOW 12 : LOW 15 : LOW 14 : RETURN
Motor_off:LOW 13 : LOW 12 : LOW 15 : LOW 14 : RETURN

Sensor 5cm

Code:
' {$STAMP BS2sx}
' {$PBASIC 2.5}
' {$PORT COM1}
ADC VAR Word
R VAR Word
I VAR Byte
X VAR Word
PAUSE 1000
HIGH 10
DO
GOSUB forward
GOSUB RD_ADC
IF ( ADC > 250 ) THEN
GOSUB backward : PAUSE 80
GOSUB motor_off : PAUSE 10000
ENDIF
LOOP
RD_ADC: LOW 10 : PAUSE 1 : HIGH 10
SEROUT 10,240,[1]
SERIN 10,240,[ADC.BYTE0,adc.BYTE1]
RETURN
Error: DEBUG "Error Reading" , CR
RETURN
forward: HIGH 13 : LOW 12 : HIGH 15 : LOW 14 : RETURN
backward: HIGH 12 : LOW 13 : HIGH 14 : LOW 15 : RETURN
S_left: HIGH 13 : LOW 12 : HIGH 14 : LOW 15 : RETURN
S_Right: HIGH 12 : LOW 13 : HIGH 15 : LOW 14 : RETURN
T_Right: LOW 13 : LOW 12 : HIGH 15 : LOW 14 : RETURN
T_LEFT: HIGH 13 : LOW 12 : LOW 15 : LOW 14 : RETURN
Motor_off:LOW 13 : LOW 12 : LOW 15 : LOW 14 : RETURN

Sensor ใกล้ที่สุด

Code:
' {$STAMP BS2sx}
' {$PBASIC 2.5}
' {$PORT COM1}
ADC VAR Word
R VAR Word
I VAR Byte
X VAR Word
PAUSE 1000
HIGH 10
DO
GOSUB forward
GOSUB RD_ADC
IF ( ADC > 385 ) THEN
GOSUB backward : PAUSE 80
GOSUB motor_off : PAUSE 10000
ENDIF
LOOP
RD_ADC: LOW 10 : PAUSE 1 : HIGH 10
SEROUT 10,240,[1]
SERIN 10,240,[ADC.BYTE0,adc.BYTE1]
RETURN
Error: DEBUG "Error Reading" , CR
RETURN
forward: HIGH 13 : LOW 12 : HIGH 15 : LOW 14 : RETURN
backward: HIGH 12 : LOW 13 : HIGH 14 : LOW 15 : RETURN
S_left: HIGH 13 : LOW 12 : HIGH 14 : LOW 15 : RETURN
S_Right: HIGH 12 : LOW 13 : HIGH 15 : LOW 14 : RETURN
T_Right: LOW 13 : LOW 12 : HIGH 15 : LOW 14 : RETURN
T_LEFT: HIGH 13 : LOW 12 : LOW 15 : LOW 14 : RETURN
Motor_off:LOW 13 : LOW 12 : LOW 15 : LOW 14 : RETURN

Touch Switch

' {$STAMP BS2sx}
' {$PBASIC 2.5}
' {$PORT COM1}
PAUSE 2000
GOSUB forward
GOSUB idea
GOSUB backward : PAUSE 400
GOSUB S_LEFT : PAUSE 190
GOSUB idea
GOSUB backward : PAUSE 400
GOSUB S_LEFT : PAUSE 190
GOSUB idea
GOSUB backward : PAUSE 400
GOSUB S_right : PAUSE 190
GOSUB idea
GOSUB backward : PAUSE 400
GOSUB S_left : PAUSE 190
GOSUB idea
GOSUB backward : PAUSE 400
GOSUB S_left : PAUSE 190
GOSUB idea
GOSUB backward : PAUSE 400
GOSUB S_right : PAUSE 190
GOSUB idea
GOSUB backward : PAUSE 400
GOSUB S_right : PAUSE 190
GOSUB idea
END
forward: HIGH 13 : LOW 12 : HIGH 15 : LOW 14 : RETURN
backward: HIGH 12 : LOW 13 : HIGH 14 : LOW 15 : RETURN
S_left: HIGH 13 : LOW 12 : HIGH 14 : LOW 15 : RETURN
S_Right: HIGH 12 : LOW 13 : HIGH 15 : LOW 14 : RETURN
T_Right: LOW 13 : LOW 12 : HIGH 15 : LOW 14 : RETURN
T_LEFT: HIGH 13 : LOW 12 : LOW 15 : LOW 14 : RETURN
Motor_off:LOW 13 : LOW 12 : LOW 15 : LOW 14 : RETURN
idea:
GOSUB forward
DO
LOOP UNTIL ( IN1 = 0 ) OR ( IN6 = 0 )RETURN


เป็นการ สั่งให้หุ่นยนต์เดินหน้าไปเรื่อยๆ เมื่อชนครั้งที่
1 จะถอยหลังแล้วเลี้ยวขวา
2 จะถอยหลังแล้วเลี้ยวขวา
3 จะถอยหลังแล้วเลี้ยวซ้าย
4 จะถอยหลังแล้วเลี้ยวขวา
5 จะถอยหลังแล้วเลี้ยวขวา
6 จะถอยหลังแล้วเลี้ยวซ้าย
7 จะถอยหลังแล้วเลี้ยวซ้าย
แล้วก็จะถึงจุดหมาย


LOOP UNTIL ( IN1 = 0 ) OR ( IN6 = 0 )
เมื่อสวิตช์ข้างซ้ายชน จะทำให้สมการ IN6=0 เป็นจริง หรือ เมื่อสวิตช์ข้างขวาชน จะทำให้สมการ IN1=0 เป็นจริง ซึ่งไม่ว่าจะชนข้างไหน
ผลที่ได้จะเหมือนกีน และจะหุ่นยนต์จะทำตามคำสั่งที่ต่อท้าย เช่น
GOSUB backward : PAUSE 400
GOSUB S_right : PAUSE 190

RoBoT tEsT

สั่งให้หุ่นยนต์: ตรงไปแล้วเดินเป็นครึ่งวงกลมไปทางซ้าย , ขวา , ซ้าย แล้วเดินตรงไป50เซนติเมตร

แล้วหมุนตัวกลับมา เดินตรงไปอีก100เซนติเมตร

แล้วหมุนตัวกลับมา

เป็นอันเสร็จสิ้น

Code:

' {$PBASIC 2.5}

' {$PORT COM2}

' {$STAMP BS2sx}

a VAR Word

b VAR Word

c VAR Word

d VAR Word

PAUSE 2000

a = 0

b = 0

c = 0

d = 0

GOSUB forward : PAUSE 1177

GOSUB S_left : PAUSE 205

DO

a = a + 1

GOSUB T_RIGHT : PAUSE 20

GOSUB FORWARD : PAUSE 15

LOOP UNTIL ( a = 75 )

DO

b = b + 1

GOSUB T_Left : PAUSE 21

GOSUB FORWARD : PAUSE 15

LOOP UNTIL ( b = 90 )

DO

c = c + 1

GOSUB T_RIGHT : PAUSE 20

GOSUB FORWARD : PAUSE 15

LOOP UNTIL ( c = 80 )

GOSUB forward : PAUSE 2344

DO

d = d + 1

GOSUB T_Right : PAUSE 20

GOSUB forward : PAUSE 5

LOOP UNTIL ( d = 50 )

GOSUB forward : PAUSE 4288

GOSUB S_RIGHT : PAUSE 370

GOSUB motor_off : PAUSE 3000

forward: HIGH 13 : LOW 12 : HIGH 15 : LOW 14 :RETURN

backward: HIGH 12 : LOW 13 : HIGH 14 : LOW 15 :RETURN

S_right: HIGH 13 : LOW 12 : HIGH 14 : LOW 15 :RETURN

S_left: HIGH 12 : LOW 13 : HIGH 15 : LOW 14 : RETURN

T_left: LOW 13 : LOW 12 : HIGH 15 : LOW 14 : RETURN

T_right: HIGH 13 : LOW 12 : LOW 15 : LOW 14 : RETURN

Motor_off:LOW 13 : LOW 12 : LOW 15 : LOW 14 : RETURN

เดินไปกลับ

โจทย์ข้อ1 โปรแกรมให้หุ่นยนต์เดินไปข้างหน้า แล้วหมุนตัว180องศา และเดินกลับมา และหมุนตัวอีก180องศา

' {$STAMP BS2sx}
' {$PBASIC 2.5}
PAUSE 1000
Main: GOSUB Forward : PAUSE 1177
GOSUB S_Right : PAUSE 350
GOSUB Forward : PAUSE 1177
GOSUB S_Right : PAUSE 300
GOSUB motor_off : PAUSE 2000
GOTO Main

forward: HIGH 13 : LOW 12 : HIGH 15 : LOW 14 : RETURN
backward: HIGH 12 : LOW 13 : HIGH 14 : LOW 15 : RETURN
S_left: HIGH 13 : LOW 12 : HIGH 14 : LOW 15 : RETURN
S_Right: HIGH 12 : LOW 13 : HIGH 15 : LOW 14 : RETURN
Motor_off:LOW 13 : LOW 12 : LOW 15 : LOW 14 : RETURN

สามเหลี่ยม

โจทย์ข้อ2 โปรแกรมให้หุ่นยนต์เดินเป็น3เหลี่ยม ด้านเท่า

' {$STAMP BS2sx}

' {$PBASIC 2.5}

PAUSE 1000

Main: GOSUB S_Right : PAUSE 90

GOSUB forward : PAUSE 1177

GOSUB S_Right : PAUSE 310

GOSUB forward : PAUSE 1177

GOSUB S_Right : PAUSE 290

GOSUB forward : PAUSE 1177

GOSUB S_Right : PAUSE 100

GOSUB motor_off : PAUSE 2000

GOTO Main

forward: HIGH 13 : LOW 12 : HIGH 15 : LOW 14 : RETURN

backward: HIGH 12 : LOW 13 : HIGH 14 : LOW 15 : RETURN

S_left: HIGH 13 : LOW 12 : HIGH 14 : LOW 15 : RETURN

S_Right: HIGH 12 : LOW 13 : HIGH 15 : LOW 14 : RETURN

Motor_off:LOW 13 : LOW 12 : LOW 15 : LOW 14 : RETURN

ครึ่งวงกลม

โจทย์ข้อ 3
' {$PBASIC 2.5}
' {$PORT COM1}
' {$STAMP BS2sx}
a VAR WordPAUSE 2000
a = 0
DO
GOSUB forward : PAUSE 1177
DO
a = a + 1
GOSUB T_RIGHT : PAUSE 20
GOSUB FORWARD : PAUSE 15
LOOP UNTIL ( a = 75 )
GOSUB FORWARD : PAUSE 1177
GOSUB motor_off : PAUSE 10000
LOOP
forward: HIGH 13 : LOW 12 : HIGH 15 : LOW 14 : RETURN
backward: HIGH 12 : LOW 13 : HIGH 14 : LOW 15 : RETURN
S_left: HIGH 13 : LOW 12 : HIGH 14 : LOW 15 : RETURN
S_Right: HIGH 12 : LOW 13 : HIGH 15 : LOW 14 : RETURN
T_Right: LOW 13 : LOW 12 : HIGH 15 : LOW 14 : RETURN
T_LEFT: HIGH 13 : LOW 12 : LOW 15 : LOW 14 : RETURN
Motor_off:LOW 13 : LOW 12 : LOW 15 : LOW 14 : RETURN