Obstacle avoidance using Occupancy grid

Hi,

I was trying to write a simple obsatcle avoidance code using an occupancy grid. My robot (a point) was suppose to do a random walk and take a 90-degree turn when it came close to a an obstacle. I do manage to create a random walk and an occupancy grid, although the obstacle avoidance loop doesn't work.

map = robotics.BinaryOccupancyGrid(20,20);
xy = [5 5; 3 2; 8 6; 10 15; 15 20; 9 8; 15 15];
setOccupancy(map,xy,1);
show(map);
hold on
grid on
maxSteps=20;
x = zeros(maxSteps);
y = zeros(maxSteps);
for i=1:20
    for j=1:20
        occval(i,j)= getOccupancy(map,[i j]);
        for stepNumber = 2: maxSteps
            if occval(i,j)==1
                x(stepNumber) = x(stepNumber-1);
                y(stepNumber) = y(stepNumber-1);
            else
                angle = 360 * rand;
                radius = 10*rand;
                x(stepNumber) = x(stepNumber-1)+radius * cosd(angle);
                y(stepNumber) = y(stepNumber-1)+radius * cosd(angle);
            end
        end
    end
end
plot(x,y)