'   {$STAMP BS2}
'   {$PBASIC 2.5}
'
' =========================================================================


' -----[ Program Description ]---------------------------------------------
'
' This program is intended to manuever the Boe-Bot using the TCS230 color sensor.
' The Boe-Bot should be able to follow any colored line with just a few adjustments.
' Currently this program allows the Boe-Bot to follow a red line to a blue line and
' return home following the red line.
'
' NOTE: OE\ must be tied low to enable TCS230 output and allow control
'       over LEDs.
'
' NOTE: To prevent sensor damage, download the program and run it before
'       connecting the TCS230.


' -----[ Revision History ]------------------------------------------------


' -----[ I/O Definitions ]-------------------------------------------------

TcsLeds         PIN     0                       ' LEDs enable - active low
TcsFreq         PIN     1                       ' freq from TCS230
TcsS2           PIN     2                       ' color filter control
TcsS3           PIN     3
Piezo           PIN     4                       ' Piezo Speaker
RightServo      PIN     12                      ' Right Servo
LeftServo       PIN     13                      ' Left Servo
PingServo       PIN     14                      ' PING))) Servo
Ping            PIN     15                      ' PING))) Sensor

' -----[ Constants ]-------------------------------------------------------

ScanTime        CON     10                      ' scan time in millisecs
ScaleMax        CON     100                     ' max for scaled values

EN              CON     1
A0              CON     2
S0              CON     3
S1              CON     4
S2              CON     5
S3              CON     6
nLED            CON     7
OUT             CON     8
pRED            CON     12
pGREEN          CON     6
pBLUE           CON     5

' -----[ Variables ]-------------------------------------------------------

irDetectLeft    VAR     Bit             ' Variable For Left IR Input
irDetectRight   VAR     Bit             ' Variable For Right IR Input
pulseCount      VAR     Byte            ' Used For Measuring Turns
distance        VAR     Word            ' Current Distance Of Object
counter         VAR     Word            ' PING))) Cycle Counter
task            VAR     Nib             ' Current Task
RED             VAR     Word
GREEN           VAR     Word
BLUE            VAR     Word

Start: 
' Initialize TAOS Sensor
  LOW A0
  HIGH S0
  HIGH S1
  LOW nLED
  HIGH EN

MainLp: 

GOSUB Color		'Check for color detected
DEBUG "R= ", RED DEC3
DEBUG "G= ", GREEN DEC3
DEBUG "B= ", BLUE DEC3
DEBUG CR

IF RED THEN
  GOSUB Forward_Pulse
  EXIT
ELSEIF BLUE THEN
  GOSUB Forward_Pulse
  ENDIF
ENDIF
		'What happens to a green detection above?
GOTO MainLp

Color: 

  LOW S2
  LOW S3
  COUNT OUT, pRED, RED
  HIGH S3
  COUNT OUT, pBLUE, BLUE
  HIGH S2
  COUNT OUT, pGREEN, GREEN

RETURN

END

' -----[ Subroutines ]-----------------------------------------------------

' *************************************************************************
' * USE THE APPROPRIATE PULSOUT VALUES TO MAKE YOUR BOE-BOT MOVE FORWARD  *
' * WHILE THE PING))) IS FACING FORWARD.                                  *
' *************************************************************************

Forward_Pulse:                          ' Send A Single Forward Pulse
  PULSOUT PingServo, 750                ' Ping Servo Forward Pulse Value
  PULSOUT LeftServo, 850                ' Left Servo Forward Pulse Value
  PULSOUT RightServo, 650               ' Right Servo Forward Pulse Value
  PAUSE 20                              ' Refresh Delay
  RETURN

' *************************************************************************
' * USE THE APPROPRIATE PULSOUT VALUES TO MAKE YOUR BOE-BOT TURN LEFT 90  *
' * DEGREES.  USE THE SAME VALUE AS ABOVE FOR THE PING))) BRACKET SERVO.  *
' *************************************************************************

Turn_Left:                              ' Left Turn, About 45 Degrees
  FOR pulseCount = 0 TO 1               ' Number Of Pulses To Turn
    PULSOUT PingServo, 750              ' Ping Servo Forward Pulse Value
    PULSOUT LeftServo, 650              ' Left Servo Left Pulse Value
    PULSOUT RightServo, 650             ' Right Servo Left Pulse Value
    PAUSE 20                            ' Refresh Delay
  NEXT
  RETURN

' *************************************************************************
' * USE THE APPROPRIATE PULSOUT VALUES TO MAKE YOUR BOE-BOT TURN RIGHT 90 *
' * DEGREES.  USE THE SAME VALUE AS ABOVE FOR THE PING))) BRACKET SERVO.  *
' *************************************************************************

Turn_Right:                             ' Right Turn, About 45 Degrees
  FOR pulseCount = 0 TO 1               ' Number Of Pulses To Turn
    PULSOUT PingServo, 750              ' Ping Servo Forward Pulse Value
    PULSOUT LeftServo, 850              ' Left Servo Right Pulse Value
    PULSOUT RightServo, 850             ' Right Servo Right Pulse Value
    PAUSE 20                            ' Refresh Delay
  NEXT
  RETURN

' *************************************************************************
' * USE THE APPROPRIATE PULSOUT VALUES TO MAKE YOUR BOE-BOT MOVE BACKWARD *
' * WHILE THE PING))) IS FACING FORWARD.                                  *
' *************************************************************************

Back_Up:                                ' Back Up
  FOR pulseCount = 0 TO 40              ' Number Of Pulses To Backup
    PULSOUT PingServo, 750              ' Ping Servo Forward Pulse Value
    PULSOUT LeftServo, 650              ' Left Servo Backup Pulse Value
    PULSOUT RightServo, 850             ' Right Servo Backup Pulse Value
    PAUSE 20                            ' Refresh Delay
  NEXT
  RETURN 