color recognition picamera

import picamera
import picamera.array
import time
import cv2

camSize = 320, 240
sampling = 4      # lower value is more sampling ie 1 samples all
minCandidates = 20      # minimun number of valid pixels for a track
searchDimension = 30
crosshairSize = 30
colorStrengthThreshold = 2
colorArray = [ [[255, 0, 0], [255, 0, 0], "red"], [[0, 255, 0], [0, 255, 0], "green"], [[0, 0, 255], [0, 0, 255], "blue"], [[0, 255, 255], [0, 255, 255], "cyan"], [[255, 255, 0], [255, 255, 0], "yellow"], [[255, 0, 255], [255, 0, 255], "magenta"], [[0, 0, 0], [0, 0, 0], "background"] ] # r, g, b, c, y, m, default
colorNames = ["red", "green", "blue", "cyan", "yellow", "magenta", "default"]
colorDistanceThreshold = 30000

trainPrompt = raw_input("Trainer mode? (y/n): ")
if trainPrompt.lower() == "y":
	for i in range(0, len(colorArray)):
		z = raw_input("train " + colorNames[i] + ": ")
		if z == "y":
			trainAvgColor = [0, 0, 0]
			# TODO: All of this can just one one initialized camera - we don't need to do this twice
			with picamera.PiCamera() as trainCam:
                            trainCam.awb_mode = 'off'
                            trainCam.awb_gains = 1.5
                            trainCam.exposure_mode = 'fixedfps'
                            trainCam.resolution = (camSize[0], camSize[1])
                            time.sleep(0.5)
                            with picamera.array.PiRGBArray(trainCam) as trainStream:
                                trainCam.capture(trainStream, resize = (camSize[0], camSize[1]), format = 'rgb')
                                trainImage = trainStream.array
                                trainPixelReadCount = 0
                                for y in range(camSize[1] / 2 - searchDimension, camSize[1] / 2 + searchDimension):
                                    for x in range(camSize[0] / 2 - searchDimension, camSize[0] / 2 + searchDimension):
                                        if x % sampling == 0 and y % sampling == 0:
                                            trainColor = trainImage[y][x]

                                            trainPixelReadCount += 1
                                            trainAvgColor[0] += trainColor[0]
                                            trainAvgColor[1] += trainColor[1]
                                            trainAvgColor[2] += trainColor[2]

                                trainAvgColor = trainAvgColor[0] / trainPixelReadCount, trainAvgColor[1] / trainPixelReadCount, trainAvgColor[2] / trainPixelReadCount

                                colorArray[i][1] = trainAvgColor


while True:
    with picamera.PiCamera() as cam:
            cam.awb_mode = 'off'
            cam.awb_gains = 1.5
            cam.exposure_mode = 'fixedfps'

            cam.resolution = (camSize[0], camSize[1])
            cam.start_preview()
            time.sleep(1)
            with picamera.array.PiRGBArray(cam) as stream:
                    cam.capture(stream, resize = (camSize[0], camSize[1]), format = 'rgb')
                    image = stream.array

                    avgColor = [0, 0, 0]
                    pixelReadCount = 0

                    for y in range(camSize[1] / 2 - searchDimension, camSize[1] / 2 + searchDimension):
                        for x in range(camSize[0] / 2 - searchDimension, camSize[0] / 2 + searchDimension):
                            if x % sampling == 0 and y % sampling == 0:
                                color = image[y][x]

                                pixelReadCount += 1
                                avgColor[0] += color[0]
                                avgColor[1] += color[1]
                                avgColor[2] += color[2]

                    avgColor = avgColor[0] / pixelReadCount, avgColor[1] / pixelReadCount, avgColor[2] / pixelReadCount

                    lastDist = colorDistanceThreshold
                    selectedColor = colorArray[6]

                    for c in colorArray:
                            colorDist = pow(avgColor[0] - c[0][0], 2) + pow(avgColor[1] - c[0][1], 2) + pow(avgColor[2] - c[0][2], 2)
                            if colorDist < lastDist:
                                    lastDist = colorDist
                                    selectedColor = c

                    print "distance: " + str(lastDist)
                    print "selected color: " + str(selectedColor[2])


    time.sleep(0.5)