'{$STAMP BS2}

 '=====Cons 
red	con	14
green	con	13
blue	con	15
cds	con	12
r	var	word
g	var	word
b	var	word
x	var	word

pause 1000
serout 9,$4054,[255,1+32,170,255,2+32,170] 'move arm to rest position
pause 1500
serout 8,$418D,[14,12,1,"Beth's M&M ColorSorter v1.2     "]
pause 5000

start:
serout 8,$418D,[14,12,1,"5 Sensor...                     "]
pause 1000
serout 8,$418D,[14,12,1,"4 Sensor...                     "]
pause 1000
serout 8,$418D,[14,12,1,"3 Sensor...                     "]
pause 1000
serout 8,$418D,[14,12,1,"2 Sensor...                     "]
pause 1000
serout 8,$418D,[14,12,1,"1 Sensor...                     "]
pause 1000
serout 8,$418D,[14,12,1,"Sensor ActivatedChecking Red    "]
high red
high cds
pause 1000
rctime cds,1,r
low red
serout 8,$418D,[14,12,1,"Sensor ActivatedChecking Green  "]
high green
high cds
pause 1000
rctime cds,1,g
low green
serout 8,$418D,[14,12,1,"Sensor ActivatedChecking Blue   "]
high blue
high cds
pause 1000
rctime cds,1,b
low blue

'debug cr
'debug ? r
'debug ? g
'debug ? b

'temp:
'r=255
'g=623
'b=1056
if r>2621 or g>2621 or b>2621 then too_dim
if r<255 and g<255 and b<255 then too_bright

r = r*25
g = g*25
b = b*25

'debug cr
'debug ? r
'debug ? g
'debug ? b

'find largest
if r>g and r>b then rlarge
if g>r and g>b then glarge
if b>r and b>g then blarge

rlarge:
x = r/255
r = r/x
g = g/x
b = b/x
goto scaling_done

glarge:
x = g/255
r = r/x
g = g/x
b = b/x
goto scaling_done

blarge:
x = b/255
r = r/x
g = g/x
b = b/x
goto scaling_done

scaling_done:
debug cr
debug ? r
debug ? g
debug ? b
debug ? x


 '=====Color Values

 'green (255 val = r)
if g>88 and g<120 and b>134 and b<195 then colorgreen

 'blue (255 val = r)
if g>112 and g<180 and b>70 and b<133 then colorblue

 'yellow (255 val = b)
if r>54 and r<95 and g>114 and g<190 then coloryellow

 'orange (255 val = b)
if r>42 and r<75 and g>191 and g<258 and b>210 and b<240 then colororange

 'red (255 val = g)
if r>54 and r<127 and b>195 and b<246 then colorred

 'brown (255 val = g)
if r>128 and r<220 and b>175 and b<220 then colorbrown

serout 8,$418D,[14,12,1,"M&M Color        = Out of range "]
pause 1000
goto start

coloryellow:
 debug "color = yellow"
 debug cr
 serout 8,$418D,[14,12,1,"M&M Color        = Yellow       "]
 gosub pickup
 serout 9,$4054,[255,0+32,137]
 gosub deliver
 serout 9,$4054,[255,0+32,197]
 pause 1000
goto start

colorgreen:
 debug "color = green"
 debug cr
 serout 8,$418D,[14,12,1,"M&M Color        = Green        "]
 gosub pickup
 serout 9,$4054,[255,0+32,102]
 gosub deliver
 serout 9,$4054,[255,0+32,197]
 pause 1000
goto start

colororange:
 debug "color = orange"
 debug cr
 serout 8,$418D,[14,12,1,"M&M Color        = Orange       "]
 gosub pickup
 serout 9,$4054,[255,0+32,154]
 gosub deliver
 serout 9,$4054,[255,0+32,197]
 pause 1000
goto start

colorred:
 debug "color = red"
 debug cr
 serout 8,$418D,[14,12,1,"M&M Color        = Red          "]
 gosub pickup
 serout 9,$4054,[255,0+32,172]
 gosub deliver
 serout 9,$4054,[255,0+32,197]
 pause 1000
goto start

colorblue:
 debug "color = blue"
 debug cr
 serout 8,$418D,[14,12,1,"M&M Color        = Blue         "]
 gosub pickup
 serout 9,$4054,[255,0+32,120]
 gosub deliver
 serout 9,$4054,[255,0+32,197]
 pause 1000
goto start

colorbrown:
 debug "color = brown"
 debug cr
 serout 8,$418D,[14,12,1,"M&M Color        = Brown        "]
 gosub pickup
 serout 9,$4054,[255,0+32,85]
 gosub deliver
 serout 9,$4054,[255,0+32,197]
 pause 1000
goto start

too_dim:
'print error to lcd
debug "too dim"
goto start

too_bright
debug "too bright"
goto start

pickup:
serout 9,$4054,[255,1+32,170,255,2+32,170] 'move arm to rest position
pause 1500
serout 9,$4054,[255,4+32,180,255,0+32,197] 'move base to P/U and open gripper
pause 1500
serout 9,$4054,[255,1+32,140,255,2+32,165,255,3+32,140] 'go to M&M
pause 1500
serout 9,$4054,[255,4+32,60] 'close gripper
pause 2500
serout 9,$4054,[255,1+32,170,255,2+32,170,255,3+32,127] 'pull back
pause 1500
return

deliver:
serout 9,$4054,[255,1+32,95,255,2+32,155,255,3+32,190]
pause 2000
serout 9,$4054,[255,4+32,180]
pause 1500
serout 9,$4054,[255,1+32,170,255,2+32,170,255,3+32,127] 'pull back
pause 1500
return