output = True
image_output = True
calibrating = False
scale_factor = 1/64.0

def debug(msg):
	global output
	if output == True:
		sys.stderr.write(str(msg) + '\n')

lab_grey_sample = (155.40740740740742, 150.55555555555554, 160.07407407407408)
lab_goal_blue = ((104, 2), (218.5, 6.5), (127.5, 14.5))

if calibrating:
	grey_sample = cvutils.calibrate_white_balance(camera)
	target_colour = cvutils.calibrate_colour_match(camera, grey_sample)
else:
	grey_sample = lab_grey_sample
	target_colour = lab_goal_blue

debug(target_colour)

def average(numbers):
	x = 0
	for num in numbers:
		x += num
	x = float(x) / len(numbers)
	return x

def x_coordinate_to_angle(coord):
	return coord*35.543
import SimpleCV
import os
import sys

sys.path.append(os.environ["HOME"] + "/robotics_repo/Documentation/Computer Vision/infoscimechatronics")
import cvutils
import time
import serial

ser = serial.Serial("/dev/serial/by-id/usb-Arduino__www.arduino.cc__0043_85431303736351D070E0-if00")

camera = SimpleCV.Camera(0, {"width": 960, "height": 540})
os.system(os.environ["HOME"] + "/robotics_repo/Projects/2017/SoccerBots/uvcdynctrl-settings.tcl")

lab_grey_sample = cvutils.calibrate_white_balance(camera)
lab_goal_blue = cvutils.calibrate_colour_match(camera, lab_grey_sample)

speed = 0
# global current_angle
# current_angle = 0
prev_error = 0
integral = 0
derivative = 0
# global hunt_dir
hunt_dir = 1
hunt_step = 15
# blobs_threshold = 0.0075
blobs_threshold = 0.0005
times = []
output = False