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);
}
