This is our final project for EE125 - Introduction Robotics in the fall semester of 2014 at UC Berkeley. We would like to thank Professor Ruzena Bacjsy, Aaron Bestick, Austin Buchan and Robert Mathew for their help.