public void avoidObstacle(double pX, double pY) { rightMotor.stop(); leftMotor.stop(); leftUltraSoundMotor.rotate(50, true); leftMotor.rotate(NavigatorUtility.convertAngle(wheelRadius, axleLength, 90), true); rightMotor.rotate(-NavigatorUtility.convertAngle(wheelRadius, axleLength, 90), false); double currentX; double currentY; do { currentX = odometer.getX(); currentY = odometer.getY(); wallFollowerController.processUSData(ultraSonicPoller.getLeftUltraSoundSensorDistance()); } while (Math.abs( NavigatorUtility.calculateAngleError(pX - currentX, pY - currentY, odometer.getTheta()) * 180 / Math.PI) > wallFollowingAngleError); leftUltraSoundMotor.rotate(-50, false); }