CMUCam4 code pid loops