Added code to setup and read from the compass module. Fixed some errors in the stabilize function.