Main cleanup

Deleted currently unused functions, variables and definitions.
Deleted readyToCalculate in motors (used when it had delay).
Setpoint thrust control in main