Made it in robotc
Updated findClosestColor, findStorePos

findClosestColor deals with z-direction now
yay high level functions
More shit