Init commit: Math, Sonar - for HC-SR04 and StateMachine wip