Make jogging and origin work as expected

[peteruithoven]

Currently jogging does not work.

• Allow to jog diagonally (this required changes to the I2C display firmware)
• Set origin does not work (nothing happens at the moment, it should set the new origin for the drawing)