Fix issue 242 (#250)

* fix #242: proposed a recoding that should solve the issue. Need test before merging into branch

* fix #242: done the test qith the device. Also fixed the zero reference of position sensor