added calibration functionality and fixed a nasty ass bug in the robot control