'///////////////////////////////////////////////////////
' BASIC Stamp program to demonstrate TCS230 RGB Color Sensor
' This program is for use with the Debug window or Windows Hyperterminal
' RGB values are reported in 10-bit format
' Performs white calibration on power-up, reset, or download
' ReadWhiteReference performs white calibration
' ReadColorSensors counts the frequency value from the TCS230
' TransmitToPC transmits the data to the PC

' This version of the program is similar to that used with the Windows
' EVM host software, except that the data is transmitted in ASCII so it
' is readable in the DEBUG window or Windows Hyperterminal.
' NOTE: This version WILL NOT work with the EVM host program unless
'	the following modifications are made.
'
'	In the TransmitToPC subroutine:
'  		Comment out the line beginning with "DEBUG"
'		Uncomment the line beginning with "SEROUT"
'	At the end of the ReadColorSensors subroutine:
'		Uncomment the 3 lines containing "/4"
'	
' NOTE: When using this program with the EVM Windows host program, reset the EVM
'	and allow the EVM to perform the white calibration before starting the 
'	host program. Otherwise the host program will fail to detect the EVM
'	because it does not communicate during white calibration.
'	
'///////////////////////////////////////////////////////

' {$Stamp bs2} ' Stamp Directive.
'{$PBASIC 2.5}

'///////////////////////////////////////////////////////
'	Variables
'	Note: For the bs2, there is a limited number of bytes
'			available for variables. 
'			Therefore, conservation is VERY important.
'///////////////////////////////////////////////////////
x				VAR	WORD	'scratch word
n				VAR	NIB	'scratch nibble
buttonWorkspace			VAR 	BYTE	'used (and required) by BUTTON call
Red				VAR	WORD	'value of red sensor of TCS230
Blue				VAR	WORD	'value of blue sensor of TCS230
Green				VAR	WORD	'value of green sensor of TCS230
Clr				VAR	WORD	'value of clear sensor of TCS230
CountRead			VAR	WORD	'number of counts read (when finding white reference)
UpperLimit			VAR	BYTE	'upper limit of bisection (when finding white reference)
LowerLimit			VAR 	BYTE	'lower limit of bisection (when finding white reference)
RedRefFreq			VAR 	BYTE	'red reference frequency (what it takes to get 255 for white)
GreenRefFreq			VAR	BYTE	'green reference frequency (what it takes to get 255 for white)
BlueRefFreq			VAR	BYTE	'blue reference frequency (what it takes to get 255 for white)
serString			VAR 	BYTE(4)	'color value as RGB bytes

'///////////////////////////////////////////////////////
'	Constants
'///////////////////////////////////////////////////////
'constants used in FindMax routine for white calibration
cRange				CON 	256	'2^m, m = 11
cBisectTries			CON 	10			
cMaxColorValue			CON	1023	'max color value, like RGB(2047,2047,2047) for whi

cDarkOffset			CON 	6

'port usage
PortEnable			CON	1	'port connected to global enable EN (active high)
PortModuleSelect		CON	2	'port connected to module select line A0
PortC230S0			CON	3	'port connected to the scaling input of the TCS230
PortC230S1			CON	4	'port connected to the scaling input of the TCS230
PortC230S2			CON	5	'port connected to the color select input of the TCS230
PortC230S3			CON	6	'port connected to the color select input of the TCS230
PortLed				CON	7	'port to turn on/off the LEDs (active low)
PortC230Freq			CON	8	'port connected to the frequency output of the TCS230


'///////////////////////////////////////////////////////
' Routine:		Boot
' Description:	This is where the program begins after
'				a hardware reset.
'///////////////////////////////////////////////////////
Boot:
	'initialize all the variables
	GOSUB InitVariables
	GOSUB InitWhiteReference

	'turn on global enable
	HIGH PortEnable

	'enable the board on J5
	LOW	PortModuleSelect

	'turn on LEDs
	Low PortLed

	'set scaling to 100%
	GOSUB Scale100

	'First do the white correct - assume target is present
	GOSUB ReadWhiteReference

'///////////////////////////////////////////////////////
' Routine:		Start
' Description:	Main program body.
'///////////////////////////////////////////////////////
Start:
'	DEBUG "Start button pressed", CR

	'turn on sensor lights
	GOSUB SensorLightOn

	'assume that the target color is in view
	GOSUB ReadColorSensors

	'transmit color to PC via COM port
	GOSUB TransmitToPC

	'turn off LEDs
	GOSUB SensorLightOff

	'wait a little
	PAUSE 500

	GOTO Start

WhiteError:
	DEBUG "Error - white surface too dark",CR

ErrLoop:GOSUB SensorLightOff
	GOTO ErrLoop

	END

'///////////////////////////////////////////////////////
'	Subroutine: SelectRed
'	Description:Select the red channel of the TSL230
'///////////////////////////////////////////////////////
SelectRed:
	'DEBUG "Select Red", CR
	LOW	PortC230S2
	LOW	PortC230S3
	RETURN

'///////////////////////////////////////////////////////
'	Subroutine: SelectGreen
'	Description:Select the green channel of the TSL230
'///////////////////////////////////////////////////////
SelectGreen:
	'DEBUG "Select Green", CR
	HIGH	PortC230S2
	HIGH	PortC230S3
	RETURN

'///////////////////////////////////////////////////////
'	Subroutine: SelectBlue
'	Description:Select the blue channel of the TSL230
'///////////////////////////////////////////////////////
SelectBlue:
	'DEBUG "Select Blue", CR
	LOW		PortC230S2
	HIGH	PortC230S3
	RETURN

'///////////////////////////////////////////////////////

'	Subroutine: SelectClear
'	Description:Select the clear channel of the TSL230
'///////////////////////////////////////////////////////
SelectClear:
	'DEBUG "Select Clear", CR
	HIGH	PortC230S2
	LOW		PortC230S3
	RETURN

'///////////////////////////////////////////////////////
'	Subroutine: FindMax
'	Description:Use bisection method to converge on 
'		a frequency that gives an output of 255.


'///////////////////////////////////////////////////////
FindMax:
	'Use bisection method to converge on the answer.
	'Start with limits of lower=0, upper=255.
	'
	'init limits
	UpperLimit = cRange - 1
	LowerLimit = 0
	n = 0

Bisect:
	'try no more than n times (Range is 2^n)
	'answer (if there is one), will be found at least in this many tries)
	n = n + 1
	IF n > cBisectTries THEN DoneFind

	'bisect limits
	x = ((UpperLimit - LowerLimit) / 2) + LowerLimit
	'DEBUG "Try ", DEC x, " limits=(", DEC LowerLimit, ",", DEC UpperLimit, ")", CR

	'read count	
	COUNT PortC230Freq,x,CountRead
	'DEBUG "CountRead = ", DEC CountRead, CR

	IF x > 10 THEN 
	  If CountRead = 0 THEN TooHigh
	ENDIF

	'remove dark offset
	if CountRead < cDarkOffset THEN TooLow
	CountRead = CountRead - cDarkOffset

	'test result
	IF CountRead = cMaxColorValue THEN DoneFind
	

	IF CountRead > cMaxColorValue THEN TooHigh
	IF CountRead < cMaxColorValue THEN TooLow

TooHigh:
	'count is too high, make current value be the new upper limit
	'DEBUG "TooHigh, set upper limit = ", DEC x, CR
	UpperLimit = x
	
	'try again
	GOTO Bisect

TooLow:
	'count is too low, make current value be the new lower limit
	'DEBUG "TooLow, set lower limit = ", DEC x, CR
	LowerLimit = x
	
	'try again
	GOTO Bisect

DoneFind:	
	'variable "x" has answer		
'	DEBUG "Result: ",DEC x, CR
	IF x >= cRange - 2 Then GOTO WhiteError

	RETURN

'///////////////////////////////////////////////////////
'	Subroutine: ReadColorSensors
'	Description:Read the current value of the color sensors.
'	Inputs:		RedRefFreq, GreenRefFreq, BlueRefFreq
'	Outputs:	Red, Green, Blue
'///////////////////////////////////////////////////////
ReadColorSensors:

ReadRed:
	'read the red
	GOSUB SelectRed					'select red
	COUNT PortC230Freq,RedRefFreq,x	'read 
	
	'remove dark offset
'	if x < cDarkOffset THEN MinRed
'	x = x - cDarkOffset

	'is value greater than max?
	if x > cMaxColorValue THEN MaxRed

	'value is in range
	Red = x
	GOTO DoneRed
MinRed:
	Red = 0
	GOTO DoneRed
MaxRed:
	'value is above range, clip it
	Red = cMaxColorValue
DoneRed:

ReadGreen:
	'read the green
	GOSUB SelectGreen					'select green
	COUNT PortC230Freq,GreenRefFreq,x	'read
	
	'remove dark offset
'	if x < cDarkOffset THEN MinGreen
'	x = x - cDarkOffset

	'is value greater than max?
	if x > cMaxColorValue THEN MaxGreen

	'value is in range
	Green = x
	GOTO DoneGreen

MinGreen:
	Green = 0
	GOTO DoneGreen
MaxGreen:
	'value is above range, clip it
	Green = cMaxColorValue
DoneGreen:

ReadBlue:
	'read the blue
	GOSUB SelectBlue					'select blue
	COUNT PortC230Freq,BlueRefFreq,x		'read
	
	'remove dark offset
'	if x < cDarkOffset THEN MinBlue
'	x = x - cDarkOffset

	'is value greater than max?
	if x > cMaxColorValue THEN MaxBlue

	'value is in range
	Blue = x
	GOTO DoneBlue

MinBlue:
	Blue = 0
	GOTO DoneBlue
MaxBlue:
	'value is above range, clip it
	Blue = cMaxColorValue
DoneBlue:

ReadColorSensorsDone:

	'Scale values back to 8 bit
' NOTE - remove comment ' from following 3 lines for operation PC host software
'	Red = Red / 4
'	Blue = Blue / 4
'	Green = Green / 4

	RETURN

'///////////////////////////////////////////////////////
'	Subroutine: ReadWhiteReference
'	Description:Find the frequency value that yields a
'				value of white (255) for each color.
'	Inputs:		None
'	Outputs:	RedRefFreq, GreenRefFreq, BlueRefFreq
'///////////////////////////////////////////////////////
ReadWhiteReference:
	'turn on sensor lights
	GOSUB SensorLightOn

	'red
	GOSUB SelectRed
	GOSUB FindMax 
'	GOSUB FineFrequencyAdjust
	RedRefFreq = x	
	
	'green
	GOSUB SelectGreen
	GOSUB FindMax 
'	GOSUB FineFrequencyAdjust
	GreenRefFreq = x	

	'blue
	GOSUB SelectBlue
	GOSUB FindMax 
'	GOSUB FineFrequencyAdjust
	BlueRefFreq = x	

	'DEBUG "White reference frequencies RGB(", DEC RedRefFreq, ",", DEC GreenRefFreq,",", DEC BlueRefFreq, ")", CR

	'turn off sensor lights
	GOSUB SensorLightOff

	RETURN

'///////////////////////////////////////////////////////
'	Subroutine: InitVariables
'	Description:Initialize the variables
'///////////////////////////////////////////////////////
InitVariables:
	Red = 0
	Blue = 0
	Green = 0
	Clr = 0
	RETURN

'///////////////////////////////////////////////////////
'	Subroutine: InitWhiteReference
'	Description:Initialize the white reference variables.
'///////////////////////////////////////////////////////
InitWhiteReference:
	CountRead = 0
	UpperLimit = 0
	LowerLimit = 0
	RedRefFreq = 0
	GreenRefFreq = 0
	BlueRefFreq	= 0
	RETURN

'///////////////////////////////////////////////////////
'	Subroutine: SensorLightOn
'	Description:Turn on sensor lights.
'///////////////////////////////////////////////////////
SensorLightOn:
	LOW PortLed
	RETURN

'///////////////////////////////////////////////////////
'	Subroutine: SensorLightOff
'	Description:Turn off sensor lights.
'///////////////////////////////////////////////////////
SensorLightOff:
	HIGH PortLed
	RETURN

'///////////////////////////////////////////////////////
'	Subroutine: Scale100
'	Description:Set scaling to 100%
'///////////////////////////////////////////////////////
Scale100:
	HIGH	PortC230S0
	HIGH	PortC230S1
	RETURN

'///////////////////////////////////////////////////////
'	Subroutine: TransmitToPC
'	Description:Transmit current color to PC via COM port.
'///////////////////////////////////////////////////////
TransmitToPC:
	'populate the RGB long word
	serString(0) = 0
	serString(1) = Red
	serString(2) = Green
	serString(3) = Blue
	
' NOTE - comment out this line for operation with PC host software
	DEBUG TAB, "Xmit RGB(", DEC Red, ",", DEC Green, ",", DEC Blue, ")", CR

	'transmit the bytes on the serial port
' NOTE - remove comment ' from following line for operation PC host software
'	SEROUT 16, 16468, [STR serString\4]

	RETURN

'///////////////////////////////////////////////////////
'	Subroutine: FineFrequencyAdjust
'	Description:Adjust frequency until it is as close to
'		to the max color value as possible.
'///////////////////////////////////////////////////////
FineFrequencyAdjust:
	'DEBUG "FineFrequencyAdjust", CR
	'DEBUG "freq =", DEC x, CR

	'decrement until count read is below max
	
Decrement:
	x = x - 1

	'read count	

	COUNT PortC230Freq,x,CountRead
	'DEBUG "x=", DEC x, "; CountRead=", DEC CountRead, CR

	'test result
	IF CountRead >= cMaxColorValue THEN Decrement
	'DEBUG "freq =", DEC x, "; CountRead = ", DEC CountRead, CR

	'increment until count read is greater than max

Increment:
	x = x + 1

	'read count	
	COUNT PortC230Freq,x,CountRead
	'DEBUG "x=", DEC x, "; CountRead=", DEC CountRead, CR

	'test result
	IF CountRead < cMaxColorValue THEN Increment
	'DEBUG "freq =", DEC x, "; CountRead = ", DEC CountRead, CR

FineFrequencyAdjustComplete:
	RETURN