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

'Mission objective:
'Students build a robot that is going in search of water ON Mars.
'Their robot needs to drive without running into objects (Chapter 7 ir navigation)
'While driving, the robot is going to look for water on mars using a dome type photoresistor
'Once the robot finds water, it will wirelessly transmit back to "Earth" that it has found water and suspend its search
'If at all possible I would love to have an array of colored objects they can look for and transmit what they found
'Ultimately it would still stop on the water
'objects are colored pieces of construction paper

'Setup:
'IR sensors are wired as they are in the book, Chapter 7 with red LEDs page 243
'pin 10, 9, and 8 have been shifted to 14, 11, 10
'have white LEDs running off 15 and 3 that provide headlights for photoresistor
'Photoresistor runs in parallel from pin 9 with 104 capaciter
'Transciever Data connected to pin 4
'Transciever Tx-Rx conected to pin 5
'Transciever Vin to VDD
'Recieving transciever wired the same
'servos in 12 and 13 (Board of education)

'-----IR Variables-----
irDetectLeft  VAR Bit
irDetectRight VAR Bit
pulseLeft     VAR Word
pulseRight    VAR Word

'-----Corner Variables-----

pulseCount   VAR  Byte
counter      VAR  Nib
old11         VAR  Bit
old0         VAR  Bit

'-----Color Detect Variables-----

light VAR Word
counterC VAR Word
colorDetect  VAR Word

'-----Transmit Variables-----
x VAR Word
y VAR Word

'-----Iniialization-----

HIGH 15   'LED for more accurate color detection
HIGH 3    'LED for more accurate color detection

'-----Main Rountine-----

Main:

  DO
    GOSUB Check_IRs
  LOOP UNTIL (irDetectLeft = 0) OR (irDetectRight = 0)

  DO
    IF (IN11 <> IN0) THEN     'Start of corner count code modified from whiskers code in book
      IF (old11 <> IN11) AND (old0 <> IN0) THEN
        counter = counter + 1
        old11 = IN11
        old0 = IN0
        IF (counter > 10) THEN
        counter = 1
        HIGH 14
        HIGH 1
        GOSUB Back_Up
        GOSUB Turn_Left
        GOSUB Turn_Left
        LOW 14
        LOW 1
      ENDIF
    ELSE
      counter = 1
    ENDIF
  ENDIF                       'End of corner count code modified from whiskers code in book

    IF (irDetectLeft = 0) AND (irDetectright = 0) THEN  'Fast navigation code from Boe-Bot book
      GOSUB Back_Up                                     'Slight modification on both detect code
      GOSUB Turn_Left
      GOSUB Turn_Left
    ELSEIF (irDetectLeft = 0) THEN
      HIGH 14
      pulseLeft = 850
      pulseRight = 850
      LOW 14
    ELSEIF (irDetectRight = 0) THEN
      HIGH 1
      pulseLeft = 650
      pulseRight = 650
      LOW 1
    ELSE
      pulseLeft = 850
      pulseRight = 650
    ENDIF

    PULSOUT 13, pulseLeft
    PULSOUT 12, pulseRight
    PAUSE 15

    GOSUB Check_IRs
    GOSUB Check_Color          'Photoresistor to look for color blue while navigating

  LOOP

'-----Subrutines-----

Check_IRs:
    FREQOUT 10, 1, 38500
    irDetectLeft = IN11
    FREQOUT 2, 1, 38500
    irDetectRight = IN0
  RETURN

Check_Color:                   'Im sure this could come out and be just check water
    GOSUB Check_Water          'was acting goofy and not driving when it wasnt like this
  RETURN

Check_Water:                   'Photoresistor code from our basic sumo bot competition
  HIGH 9
  PAUSE 2
  RCTIME 9, 1, light
  DEBUG HOME, DEC5 light
    IF light > 800 THEN RETURN
    IF light < 600 THEN RETURN
      HIGH 14
      HIGH 1
      PAUSE 10000
      LOW 14
      LOW 1
        GOSUB Transmit        'how do i get this to not transmit the whole time?
        GOSUB Drive_Forward   'would prefer that they stop here. want the robot to stay at the "water"
    RETURN

Back_Up:
  FOR pulseCount = 0 TO 7
    PULSOUT 13, 650
    PULSOUT 12, 850
    PAUSE 20
  NEXT
  RETURN

Turn_Left:
  FOR pulseCount = 0 TO 15
    PULSOUT 13, 650
    PULSOUT 12, 650
    PAUSE 20
  NEXT
  RETURN

Drive_Forward:
  FOR pulseCount = 0 TO 10
    PULSOUT 13, 850
    PULSOUT 12, 650
    PAUSE 20
  NEXT
  RETURN

Transmit:                            'This was a basic transmit code from the 433MHz RF Transceiver (#27982) PDF
  HIGH 5

  PULSOUT 4, 1200
  SEROUT 4, 16468, [ "!", x.HIGHBYTE, x.LOWBYTE, y.HIGHBYTE, y.LOWBYTE ]
  x = x + 1
  y = y + 1
  PAUSE 10                            'How do i control an LED on the recieving robot or make it debug "We Found Water"??
RETURN