Code refactor.

Move lean angle calculation from ForkMotorController to ControlLoop.