Add motor cutoff when power is out.
Add simple PID controller.
Add gyro module
Use doubles and hook in the PID controller.