Changed the coding of the roll and pitch shift parameters.
Modified the upstairs algorithm to work in the real robot.