First implementation

Implementaiton of calculaitons for scara-type printers.
Not tested but compiles without errors.
Calubraiton is not implemented yet! Don't use on real printers until
completed.