' {$STAMP BS2}
' {$PBASIC 2.5}
' {$PORT COM1}

IR_IN           PIN     8                       'IR Remote 接收遙控器的 PIN
time VAR Word(4)                                'For IR signal storage
direction           VAR     Byte

ch VAR Byte
pw1 VAR Word
pw2 VAR Word
pw3 VAR Word
pw4 VAR Word
pw5 VAR Word
ra VAR Byte
Sdat CON 15
baud CON 396

ra = 7
pw1=750
pw2=750
pw3=750
pw4=750
pw5=750

SEROUT Sdat, Baud+$8000,["!SC", 1, ra, pw1.LOWBYTE, pw1.HIGHBYTE, CR]
PAUSE 500
SEROUT Sdat, Baud+$8000,["!SC", 2, ra, pw1.LOWBYTE, pw1.HIGHBYTE, CR]
PAUSE 500
SEROUT Sdat, Baud+$8000,["!SC", 3, ra, pw1.LOWBYTE, pw1.HIGHBYTE, CR]
PAUSE 500
SEROUT Sdat, Baud+$8000,["!SC", 4, ra, pw1.LOWBYTE, pw1.HIGHBYTE, CR]
PAUSE 500
SEROUT Sdat, Baud+$8000,["!SC", 5, ra, pw1.LOWBYTE, pw1.HIGHBYTE, CR]
PAUSE 500

DO

DO
  RCTIME IR_IN, 1, time(0) ' 此段程式用來接收TV遙控器送過來的調變訊號, 此段程式只適用在SONY遙控器
LOOP UNTIL time(0) > 1000  ' 只需抓前4個data就夠用來區別遙控器的0~9, 所以在此只有設time(0~3), 4 組 ARRAY

  PULSIN IR_IN, 0, time(0) ' Measure/store data pulses.
  PULSIN IR_IN, 0, time(1)
  PULSIN IR_IN, 0, time(2)
  PULSIN IR_IN, 0, time(3)
  'DEBUG  DEC(time(0)), CR '以下讀出 TV遙控按鈕的值
  'DEBUG  DEC(time(1)), CR
  'DEBUG  DEC(time(2)), CR
  'DEBUG  DEC(time(3)), CR
  'DEBUG "----------------",CR



IF (time(3) < 650 AND time(3) > 550 ) AND (time(2) < 400) AND (time(2) > 300) AND (time(1) < 400) AND (time(1) > 300) AND (time(0) < 400) AND (time(0) > 300) THEN '9' Heading to 4 O'clock
  'pw0 = 900
  'pw2 = 773
  'pw1 = 520
  'GOSUB ServoMove
  DEBUG "9 axis5 cw",CR
  ch=5
  direction =9


ELSEIF (time(3) < 690 AND time(3) > 600 ) AND (time(2) < 400) AND (time(2) > 310) AND (time(1) < 400) AND (time(1) > 310) AND (time(0) < 690) AND (time(0) > 610) THEN     '1'
  'pw0 = 520
  'pw1 = 870
  'pw2 = 773
  'GOSUB ServoMove
  DEBUG "0 axis5 ccw",CR
  ch=5
  direction =0


ELSEIF (time(3) < 400 AND time(3) > 300 ) AND (time(2) < 400) AND (time(2) > 300) AND (time(1) < 400) AND (time(1) > 300) AND (time(0) < 400) AND (time(0) > 300) THEN     '1'
  'pw0 = 520
  'pw1 = 870
  'pw2 = 773
  'GOSUB ServoMove
  DEBUG "1 axis1 cw",CR
  ch=1
  direction =1


ELSEIF (time(3) < 380 AND time(3) > 330 ) AND (time(2) < 390) AND (time(2) > 320) AND (time(1) < 380) AND (time(1) > 320) AND (time(0) < 680 AND time(0) > 610) THEN     '2'
  'pw2 = 520
  'pw1 = 870
  'pw0 = 773
  'GOSUB ServoMove
  DEBUG "2 axis1 ccw",CR
  ch=1
  direction =2

ELSEIF (time(3) < 400 AND time(3) > 300 ) AND (time(2) < 400) AND (time(2) > 300) AND (time(1) < 650 AND time(1) > 550) AND (time(0) < 400) AND (time(0) > 300) THEN     '3'
  'pw0 = 900
  'pw1 = 773
  'pw2 = 520
  'GOSUB ServoMove
  DEBUG "3 axis2 cw",CR
  ch=2
  direction =3



ELSEIF (time(3) < 380 AND time(3) > 320 ) AND (time(2) < 370) AND (time(2) > 320) AND (time(1) < 680 AND time(1) > 630) AND (time(0) < 670 AND time(0) > 610) THEN     '4' Turn Left
  'pw0 = 670
  'pw1 = 670
  'pw2 = 670
  'GOSUB ServoMove
  DEBUG "4 axis2 ccw",CR
  ch=2
  direction =4

ELSEIF (time(3) < 400 AND time(3) > 300 ) AND (time(2) < 650 AND time(2) > 550) AND (time(1) < 400) AND (time(1) > 300) AND (time(0) < 400) AND (time(0) > 300) THEN     '5'
  'pw0 = 773
  'pw1 = 773
  'pw2 = 773
  'GOSUB ServoMove
  DEBUG "5 axis3 cw",CR
  ch=3
  direction =5


ELSEIF (time(3) < 390 AND time(3) > 340 ) AND (time(2) < 690 AND time(2) > 630) AND (time(1) < 395) AND (time(1) > 325) AND (time(0) < 685 AND time(0) > 620) THEN     '6' Turn Right
  'pw0 = 870
  'pw1 = 870
  'pw2 = 870
  'GOSUB ServoMove
  DEBUG "6 axis3 ccw",CR
  ch=3
  direction =6


ELSEIF (time(3) < 400 AND time(3) > 300 ) AND (time(2) < 650 AND time(2) > 550) AND (time(1) < 650 AND time(1) > 550) AND (time(0) < 400) AND (time(0) > 300) THEN     '7' Heading to 8 O'clock
  'pw0 = 520
  'pw1 = 773
  'pw2 = 920
  'GOSUB ServoMove
  DEBUG "7 axis4 cw",CR
  ch=4
  direction =7

ELSEIF (time(3) < 380 AND time(3) > 330 ) AND (time(2) < 670 AND time(2) > 630) AND (time(1) < 670 AND time(1) > 630) AND (time(0) < 680 AND time(0) > 630) THEN     '8' Backward
  'pw1 = 520
  'pw2 = 870
  'pw0 = 773
  'GOSUB ServoMove
  DEBUG "8 axis4 ccw",CR
  ch=4
  direction =8

ELSE
direction =-1  'servos no move

ENDIF

SELECT direction
CASE 1
ch=1
pw1 = pw1 + 30  'axis 1
SEROUT Sdat, Baud+$8000,["!SC", ch, ra, pw1.LOWBYTE, pw1.HIGHBYTE, CR]
DEBUG DEC(pw1),CR
PAUSE 200

CASE 2
pw1 = pw1 - 30
SEROUT Sdat, Baud+$8000,["!SC", ch, ra, pw1.LOWBYTE, pw1.HIGHBYTE, CR]
DEBUG DEC(pw1),CR
PAUSE 200

CASE 3
pw2 = pw2 + 30   'axis 2
SEROUT Sdat, Baud+$8000,["!SC", ch, ra, pw2.LOWBYTE, pw2.HIGHBYTE, CR]
DEBUG DEC(pw2),CR
PAUSE 200

CASE 4
pw2 = pw2 - 30
SEROUT Sdat, Baud+$8000,["!SC", ch, ra, pw2.LOWBYTE, pw2.HIGHBYTE, CR]
DEBUG DEC(pw2),CR
PAUSE 200

CASE 5
pw3 = pw3 + 30   'axis 3
SEROUT Sdat, Baud+$8000,["!SC", ch, ra, pw3.LOWBYTE, pw3.HIGHBYTE, CR]
DEBUG pw3
PAUSE 200

CASE 6
pw3 = pw3 - 30
SEROUT Sdat, Baud+$8000,["!SC", ch, ra, pw3.LOWBYTE, pw3.HIGHBYTE, CR]
DEBUG pw3
PAUSE 200

CASE 7
pw4 = pw4 + 30   'axis 4
SEROUT Sdat, Baud+$8000,["!SC", ch, ra, pw4.LOWBYTE, pw4.HIGHBYTE, CR]
PAUSE 200

CASE 8
pw4 = pw4 - 30
SEROUT Sdat, Baud+$8000,["!SC", ch, ra, pw4.LOWBYTE, pw4.HIGHBYTE, CR]
PAUSE 200

CASE 9
pw5 = pw5 + 30   'axis 5
SEROUT Sdat, Baud+$8000,["!SC", ch, ra, pw5.LOWBYTE, pw5.HIGHBYTE, CR]
PAUSE 200

CASE 0
pw5 = pw5 - 30
SEROUT Sdat, Baud+$8000,["!SC", ch, ra, pw5.LOWBYTE, pw5.HIGHBYTE, CR]
PAUSE 200

ENDSELECT
LOOP