lastest code (incomplete)

Still fixing the calibration problem