Added robot calibrate subroutine
Added robot calibrate subroutine