'{$STAMP BS2}
'{$PBASIC 2.5}

counter  VAR  Word

'Define port pins

EN   CON  1
S2   CON  5
S3   CON  6
OUT  CON  8
M1   CON  9
M2   CON 10
Led1 CON 11
Led2 CON 12

'Define count periods for each color.
'Adjust these for a reading just under 255
'  for a white sheet of paper.

pRED    CON  12
pGREEN  CON  8
pBLUE   CON  6

'Define variables for color results.

RED    VAR  Word    'varibale that stores the value of RED that the optical sensor sees
GREEN  VAR  Word    'varibale that stores the value of GREEN that the optical sensor sees
BLUE   VAR  Word    'varibale that stores the value of BLUE that the optical sensor sees
MandM  VAR  Nib     'The M&M color.

'Program starts here.

Initialization:
  HIGH  Led1
  HIGH  Led2

Main:
DO
      GOSUB Color                        'Get the color data.
      GOSUB WhatColor                    'Determines the color of the M&M
  IF MandM = 7 THEN
    GOSUB Forward_Pulse
  ELSEIF MandM <> 7 THEN
    GOSUB Search
  ENDIF
LOOP

'Color: Read all three color components.

Color: LOW  S2                  'Address the red output.
  LOW  S3
  COUNT  OUT, pRED, RED         'Read the red component.
  HIGH  S3                      'Address the blue output.
  COUNT  OUT, pBLUE, BLUE       'Read the blue component.
  HIGH  S2                      'Address the green output.
  COUNT  OUT, pGREEN, GREEN     'Read the green component.
RETURN

'WhatColor: determines the color of the M&M
'  See specs for details
WhatColor:
  IF (RED>=240) AND (BLUE>=40) AND (BLUE<=210) AND (GREEN>=170) AND (GREEN<=223) THEN
     MandM = 1
     DEBUG "Yellow M&M"
  ELSEIF (RED>=30) AND (RED<=50)AND (BLUE>=10) AND (BLUE<=33) AND (GREEN>=20) AND (GREEN<=43) THEN
     MandM = 2
     DEBUG "Brown M&M"
  ELSEIF (RED>=20) AND (RED<=39)AND (BLUE>=80) AND (BLUE<=99) AND (GREEN>=40) AND (GREEN<=59) THEN
     MandM = 3
     DEBUG "Blue M&M"
  ELSEIF (RED>=120) AND (RED<=160)AND (BLUE>=0) AND (BLUE<=39) AND (GREEN>=10) AND (GREEN<=46) THEN
     MandM = 4
     DEBUG "Red M&M"
  ELSEIF (RED>=165) AND (BLUE>=10) AND (BLUE<=32) AND (GREEN>=50) AND (GREEN<=73) THEN
     MandM = 5
     DEBUG "Orange M&M"
  ELSEIF (RED>=40) AND (RED<=57)AND (BLUE>=40) AND (BLUE<=62) AND (GREEN>=100) AND (GREEN<=122) THEN
     MandM = 6
     DEBUG "Green M&M"
  ELSEIF (RED<=10)AND (BLUE<=10) AND (GREEN<=10) THEN
     MandM = 7
     DEBUG "No M&M"
  ENDIF
  RETURN

  Forward_Pulse:
  FOR counter = 1 TO 6
    PULSOUT M1, 850
    PULSOUT M2, 650
  NEXT
  RETURN

  Search:
  PAUSE 1000
  FOR counter = 1 TO 50
    PULSOUT M1, 850
    PULSOUT M2, 850
  NEXT
  RETURN 