implemented SCARA-Code and Maths from QHarley

added some #ifdenfines