Small bugfix, changed values in helper to consider new gearbox
Bugfix!!! Convert angle to rad for cos and sin
Fine tuning values
Removed TODO