SensorModule update

Started to write the position calibration function but it is not finished yet. It's only rough implemented (just basic idea) but rest still works so it can be used.