InitColorSensorService:

Call UpdateColor to assign LastColor

Initialize the update timer

RunColorSensorService:
If this event is ES_TIMEOUT from update timer
Re-initialize the timer
If we are transporting the miner
Query the miner location of the current miner
If this location is invalid
Update the current color of tractor
Else
Update the current color of tractor
End if
End if
End if

QueryColor:
Return last known color

SetDesiredColor:
Set color for desired color

CheckColor: (Event Checker)

If the current color does not match the last color
If the current color is the desired color
Post TargetSquareReached to NavigationSM
Reset the desired color
End if
Set return value to 1
End if
Update the LastColor
If CurrentColor is valid
Update last known color
End if
Return return value

UpdateColor:
Call GetRGB
Call RBG2HSV
Return ClassifyColor

GetRGB:
Query RGB and clear values from I2C module

RBG2HSV:
Compute HSV values from RBG using running average

ClassifyColor
Loop through the colors
If color falls within specific HSV range and tolerance
Return that color
End if