Code sometimes getting stuck in an infinite loop when changing target point for 2d simulation of robotic arm. HELP

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% clc; clear;

pt = [1;2]; % target point (in column matrix) l1 = 1; l2 = 1; l3 = 1; l4 = 1; theta1 = pi/6; theta2 = pi/6; theta3 = pi/6; theta4 = pi/6; dt =.01; %time step counter = 0; figure(1) title('Planar Robot') axis manual % set axis to exact manual value( hold on % does not erase previous graphs %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%% initialization of data num_of_link = 4; %number of link xdata = (0:l1+l2+l3+l4);

for i = 1:num_of_link+1 ydata(i) = 0; end for i = 1:num_of_link angledata(i) = 0; end

error = dist([xdata(num_of_link+1) ydata(num_of_link+1)], pt); while (error > 0.5) iteration = num_of_link + 1; while (iteration > 1) %% CCD Algorthm

        pe = [xdata(num_of_link+1); ydata(num_of_link+1)];
        pc = [xdata(iteration-1); ydata(iteration-1)];

        a = (pe - pc)/norm(pe-pc);
        b = (pt - pc)/norm(pt-pc);
        theta = acosd(dot(a, b));

        direction = cross([a(1) a(2) 0],[b(1) b(2) 0]);
        if direction(3) < 0
            theta = -theta; %defines whether or not it's a minus or positive value
        end

         if (theta > 30)
            theta = 30;
        elseif (theta < -30)
            theta = -30;
        end

        angledata(iteration-1) = theta;

        iteration = iteration - 1;

        %%let's do the rotation!
        for i = 1:num_of_link-1
            R = [cosd(angledata(i)) -sind(angledata(i)); sind(angledata(i)) cosd(angledata(i))]; % rotation matrix
            % p' = R * (p - c) + c
            temp = R * ([xdata(i+1); ydata(i+1)] - [xdata(i); ydata(i)]) + [xdata(i); ydata(i)];
            xdata(i+1) = temp(1);
            ydata(i+1) = temp(2);
            angledata(i+1) = angledata(i+1) + angledata(i);
            xdata(i+2) = xdata(i+1)+1;
            ydata(i+2) = ydata(i+1);
        end

        % end effector rotation
        i = i+1;
        R = [cosd(angledata(i)) -sind(angledata(i)); sind(angledata(i)) cosd(angledata(i))]; % rotation matrix
        % p' = R * (p - c) + c
        temp = R * ([xdata(i+1); ydata(i+1)] - [xdata(i); ydata(i)]) + [xdata(i); ydata(i)];
        xdata(i+1) = temp(1);
        ydata(i+1) = temp(2);

    end
    error = dist([xdata(num_of_link+1) ydata(num_of_link+1)], pt);
    disp(error);
end

%Calculate the location of the middle three joints pointl1 = [l1*cos(theta1) ; l1*sin(theta1)]; pointl2 = pointl1 + [l2*cos(theta1+theta2);l2*sin(theta1+theta2)]; pointl3 = pointl2 + [l3*cos(theta1+theta2+theta3);l3*sin(theta1+theta2+theta3)];

%Plot if (mod(counter,10)==0) %plots every 10 iterations axis([0 10 0 10]) axis square line([0,pointl1(1)],[0,pointl1(2,1)]) hold on line([pointl1(1),pointl2(1)],[pointl1(2,1),pointl2(2,1)]) hold on line([pointl2(1),pointl3(1)],[pointl2(2,1),pointl3(2,1)]) line([pointl3(1),temp(1)],[pointl3(2,1),temp(2,1)])

plot(temp(1),temp(2),'o') pause(.1)

end

disp('Simulate');