Fixed rotation (so it is mostly working...) and added zero point finder