Consistency in theta definitions, reduced arm DOF

Made sure all parts of the code were rotating consistently and changed
optimization code to begin search for thetas from last known thetas.
Reduced optimization time from 4 seconds to ~0.7 seconds. Needs to be
faster still.
Added Serial port communication for Simulation

Can now use the simulation with the IMU.
Merge pull request #1 from gbledt/master

IMU added to simulation