Reset IMU angle when control is (re)enabled
If the robot falls, the angle is off, due to the gyroscope saturating. When the PID controllers are restarted, a new initial measurement should be done. The same code (function) can be used to init the gyro at power-on.