added sensor calibration, calculating motor positions code