def __init__(self):
self.IMAGE_WIDTH = 320
self.IMAGE_HEIGHT = 240
# PID parameters
self.KP = 0.035
self.KI = 0.045
self.KD = 0.005
self.prev_errx = 0
self.prev_erry = 0
self.integral_x = 0
self.integral_y = 0
self.curr_yaw = 90
self.curr_pitch = 90
self.last_obs = time.time()
# Open the camera
self.capture = cv.CreateCameraCapture(0)
cv.SetCaptureProperty(self.capture, cv.CV_CAP_PROP_FRAME_WIDTH,
self.IMAGE_WIDTH)
cv.SetCaptureProperty(self.capture, cv.CV_CAP_PROP_FRAME_HEIGHT,
self.IMAGE_HEIGHT)
# Union-Find Connected Comonent Labeling
self.detector = BlobDetector()
# Kalman filter
self.filter = KalmanFilter()
# Open the serial port to the arduino
print 'Opening serial port ...'
self.serial = serial.Serial('/dev/ttyACM0', 19200)
time.sleep(2)
print 'Moving servos to initial position ...'
self.serial.write('90s90t')
time.sleep(1)
