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

rcv VAR Byte(10)
pos0 VAR Byte
pos1 VAR Byte
x VAR Byte
temp VAR Word
ziggy VAR Nib(2)
order VAR Nib(4)
joey VAR Nib
backward VAR Byte(2)

maxjoey VAR Nib
joey = 0

DEBUG CR,"Number?",CR
DEBUGIN maxjoey
DEBUG CR,"Order?",CR
FOR x=0 TO maxjoey-1
DEBUGIN order(x)
NEXT

farleft VAR Byte

OUTPUT 5
OUTPUT 15

INPUT 0

'Stop movement
LOW 5
LOW 15

initialize:

'Lift shaft
FOR x=0 TO 100 
PULSOUT 12,1000
PAUSE 10
NEXT

'Record initial Position
RCTIME 2,1,pos0
LOW 2
HIGH 2
DEBUG CLS,DEC pos0

pos0 = 1000

HIGH 15 'Move left

'Wait for limit switch to be pressed
DO WHILE(IN0 = 0)
RCTIME 2,1,pos0
LOW 2
HIGH 2
PAUSE 60
DEBUG CLS,DEC pos0
LOOP

farleft = pos0

LOW 15

PAUSE 1000

'Initialize camera (see determine color for details)
SEROUT 10, 84, ["RS",CR]
SERIN 9, 84, [WAIT (":")]
PAUSE 1000
DEBUG "Resetting camera, preparing to auto-adjust light level",CR

SEROUT 10, 84, ["L1 1",CR]
SERIN 9, 84, [WAIT (":")]
PAUSE 100


SEROUT 10, 84, ["CR 18 44",CR]
SERIN 9, 84, [WAIT (":")]
PAUSE 5100


SEROUT 10, 84, ["CR 18 44 19 32",CR]
SERIN 9, 84, [WAIT (":")]
PAUSE 100
DEBUG "Light level adjusted, setting data transfer mode",CR,CR

SEROUT 10, 84, ["L1 2", CR]
SERIN 9, 84, [WAIT (":")]
PAUSE 100


PAUSE 5000

SEROUT 10, 84, ["PM 1",CR]
SERIN 9, 84, [WAIT (":")]
PAUSE 100


SEROUT 10, 84, ["RM 3",CR]
PAUSE 100
SEROUT 10, 84, ["TW",CR]

SERIN 9, 84, [STR rcv\8]
PAUSE 100

DEBUG "Red Mean ",DEC rcv(2) ,CR
DEBUG "Green Mean ",DEC RCV(3) ,CR
DEBUG "Blue Mean ",DEC RCV(4) ,CR
DEBUG "Red Deviation ",DEC RCV(5) ,CR
DEBUG "Green Deviation ",DEC RCV(6) ,CR
DEBUG "Blue Deviation ",DEC RCV(7) ,CR
DEBUG " ", CR

SEROUT 10,84,["TC",CR]
SERIN 9, 84, [STR rcv\10]
PAUSE 100

DEBUG "Middle Mass X ",DEC RCV(2) ,CR
DEBUG "Middle Mass Y ",DEC RCV(3) ,CR
DEBUG "Left Corner X ",DEC RCV(4) ,CR
DEBUG "Left Corner Y ",DEC RCV(5) ,CR
DEBUG "Right corner X ",DEC RCV(6) ,CR
DEBUG "Right Corner Y ",DEC RCV(7) ,CR
DEBUG "Pixels ",DEC RCV(8) ,CR
DEBUG "Confidence ",DEC RCV(9) ,CR
DEBUG " ", CR
HIGH 5

seekandlift:

'Update position
RCTIME 2,1,pos0
LOW 2
HIGH 2
DEBUG CLS,DEC pos0

'Probe for colors
SEROUT 10,84,["TC 0 110 0 110 110 255",CR]
'pause 100
SERIN 9, 84, [STR rcv\10]
PAUSE 12
IF (rcv(9) >= 75) THEN GOTO official_b

SEROUT 10,84,["TC 0 130 130 255 0 130",CR]
'pause 100
SERIN 9, 84, [STR rcv\10]
PAUSE 12
IF (rcv(9) >= 75) THEN GOTO official_g

SEROUT 10,84,["TC 100 255 0 45 0 45",CR]
'PAUSE 100
SERIN 9, 84, [STR rcv\10]
PAUSE 12
IF (rcv(9) >= 75) THEN GOTO official_r

SEROUT 10, 84, ["TC 180 255 70 180 0 50",CR]
'pause 100
SERIN 9, 84, [STR rcv\10]
PAUSE 12
IF(rcv(9) >= 75) THEN official_o

jigg:

PAUSE 60

GOTO seekandlift

official:
'See if color is what we're looking for
IF(NOT (order(joey) = ziggy(1))) THEN seekandlift
joey = joey + 1
LOW 5
LOW 15

'Pick up and move
HIGH 15
pos1 = 1000
DO WHILE(pos1 > pos0 - 25)
RCTIME 2,1,pos1
LOW 2
HIGH 2
DEBUG CLS,DEC pos1',CR,"Red conf: ",DEC rcv(9)
PAUSE 60
LOOP
LOW 15

meesh:
FOR x=0 TO 100
PULSOUT 12,500
PAUSE 10
NEXT

HIGH 5
temp = pos1
DO WHILE(pos1 <= temp + 14)
RCTIME 2,1,pos1
LOW 2
HIGH 2
DEBUG CLS,DEC pos1
PAUSE 60
LOOP

LOW 5

FOR x=0 TO 100
PULSOUT 12,1000
PAUSE 10
NEXT

HIGH 15
temp = pos1
DO WHILE(pos1 >= temp - 45)
RCTIME 2,1,pos1
LOW 2
HIGH 2
DEBUG CLS,DEC pos1
PAUSE 60
LOOP

LOW 15

FOR x=0 TO 100
PULSOUT 12,500
PAUSE 10
NEXT

HIGH 15
temp = pos1
DO WHILE(pos1 >= temp - 5)
RCTIME 2,1,pos1
LOW 2
HIGH 2
DEBUG CLS,DEC pos1
PAUSE 60
LOOP
LOW 15

IF((joey > maxjoey - 1)) THEN GOTO done

FOR x=0 TO 100
PULSOUT 12,1000
PAUSE 10
NEXT
HIGH 5
GOTO seekandlift

done:

END

official_r:
ziggy(1) = 0
PAUSE 30
GOTO official

official_o:
ziggy(1) = 1
PAUSE 15
GOTO official

official_b:
ziggy(1) = 2
PAUSE 4
GOTO official

official_g:
ziggy(1) = 3
GOTO official