Keep trying to calibrate gyro until it is standing still
Call stopAndReset() in setup

Closes #1
Use constrain to limit restAngle
Updated submodules
Updated hex