Added test-code

Insert this into MArlin_main.cpp to test new_mathematics with command
M998