Implemented SCARA-Maths

Tested and reviewed by father of REPRAP Scara Quinton Harley
really #ifdef ?