Inverse kinematics function not working properly [GUIDE]

I have a robot with the following DH parameters:

d = [0.256 0 0 0 0.159];
a = [0 0.256 0.256 0 0];
alpha = [-pi/2 0 0 -pi/2 0];

I am using Robotics Toolbox - Peter Corke to plot my robot, using the same DH parameters above. The code I use for plotting the robot using this addon in matlab is the following:

L(1) = Link('revolute', 'd', d(1), 'a', a(1), 'alpha', alpha(1));
L(2) = Link('revolute', 'd', d(2), 'a', a(2), 'alpha', alpha(2));
L(3) = Link('revolute', 'd', d(3), 'a', a(3), 'alpha', alpha(3));
L(4) = Link('revolute', 'd', d(4), 'a', a(4), 'alpha', alpha(4));
L(5) = Link('revolute', 'd', d(5), 'a', a(5), 'alpha', alpha(5));
robot = SerialLink(L);
robot.name = 'CRS Catalyst 5';

This is my forward kinematics code:

% Forward Kinematics
T = @(theta, alpha, a, d) [cos(theta) -sin(theta)*cos(alpha) sin(theta)*sin(alpha) a*cos(theta);
                          sin(theta) cos(theta)*cos(alpha) -cos(theta)*sin(alpha) a*sin(theta);
                          0 sin(alpha) cos(alpha) d;
                          0 0 0 1];

T01 = T(t1, alpha(1), a(1), d(1));
T12 = T(t2, alpha(2), a(2), d(2));
T23 = T(t3, alpha(3), a(3), d(3));
T34 = T(t4, alpha(4), a(4), d(4));
T45 = T(0, alpha(5), a(5), d(5));

T05 = T01 * T12 * T23 * T34 * T45;

% Display End Effector Position
disp("End Effector Position:");
disp(T05(1:3, 4)');

These are my inverse kinematics code, I have to versions to see what is going on:

First version:

phi = 90;
theta234 = pi-phi;

R = d(5)*cos(phi);
Px_w = Px - R*cos(theta(1));
Py_w = Py - R*sin(theta(1));

q1_1 = round(rad2deg(atan2(Py,Px)),3)
q1_2 = round(rad2deg(q1_1 + 180),3)

Pz_w = Pz + d(5)*sin(phi);
R_w = sqrt(Px_w^2 + Py_w^2);

N = sqrt((Pz_w - d(1))^2 + R_w^2);
cosu = (N^2+a(2)^2-a(3)^2)/(2*a(2)*N)
u = acos((N^2+a(2)^2-a(3)^2)/(2*a(2)*N));
lambda = atan2((Pz_w-d(1)),R_w);
q2_1 = rad2deg(lambda + u)
q2_2 = rad2deg(lambda - u)
cosq3 = ((N^2-a(2)^2-a(3)^2)/2*a(2)*a(3))
q3_1 = rad2deg(acos(((N^2-a(2)^2-a(3)^2)/2*a(2)*a(3))-pi))
q3_2 = rad2deg(-acos(((N^2-a(2)^2-a(3)^2)/2*a(2)*a(3))-pi))

q4_1 = theta234 - q2_1 - q3_1
q4_2 = theta234 - q2_2 - q3_2
q4_3 = theta234 - q2_2 - q3_1
q4_4 = theta234 - q2_1 - q3_1

This first version of my code was based on Inverse Kinematics Analysis and Simulation of a 5 DOF Robotic Arm using MATLAB , especifically with the following data:

Second version is basically a mix of what I have found online and through this research paper:

function [theta1, theta2, theta3, theta4] = inv_kinematics(x, y, z)

d = [0.256 0 0 0 0.159];
a = [0 0.256 0.256 0 0];
alpha = [-pi/2 0 0 -pi/2 0];
theta4 = 0; % Constant

% Calculate theta1
theta1 = atan2(y, x);

% Calculate distance between the first and the end-effector in the xy-plane
d_xy = sqrt(x^2 + y^2) - a(1);

% Calculate distance between the first and the end-effector in the z-direction
d_z = z - d(1);

% Calculate distance between the second and the end-effector in the xy-plane
d_xy_2 = sqrt(d_xy^2 + d_z^2);

% Calculate angle between the second and the end-effector in the xy-plane
alpha_2 = atan2(d_z, d_xy);

% Calculate angle between the second and the third link
cos_theta3 = (d_xy_2^2 + d(3)^2 - a(2)^2 - a(3)^2) / (2*a(2)*a(3));
sin_theta3 = sqrt(1 - cos_theta3^2);
theta3 = atan2(real(sin_theta3), real(cos_theta3)); %%%%%%%

% Calculate angle between the first and the third link in the xy-plane
cos_beta = (a(2)^2 + d_xy_2^2 - a(3)^2) / (2*a(2)*d_xy_2);
sin_beta = sqrt(1 - cos_beta^2);

% Calculate angle between the first and the third link in the xyz-plane
alpha_3 = atan2(d_z, d_xy) + atan2(real(sin_beta), real(cos_beta));  %%%%%%%

% Calculate angle between the third and the fourth link
cos_theta2 = (a(2)^2 + a(3)^2 - d_xy_2^2) / (2*a(2)*a(3));
sin_theta2 = sqrt(1 - cos_theta2^2);
theta2 = atan2(real(sin_theta2), real(cos_theta2)); %%%%%%%%%%%%

% Calculate angle between the fourth and the fifth link
theta5 = -alpha_3 - theta2 - theta3;

% Return the joint angles
theta1 = wrapToPi(theta1);
theta2 = wrapToPi(theta2- pi); % Convert to -pi/2 to pi/2 range
theta3 = wrapToPi(theta3); % Convert to -pi/2 to pi/2 range
theta4 = wrapToPi(theta4); % Convert to -pi/2 to pi/2 range
end

The problem is that my results obtained through inverse kinematics function doesn't give me the same end-effector position that I inserted into the inverse kinematics function. The second version of code for inverse kinematics sort of works, but at certain points only. Also note that theta5, is for the wrist to rotate on its own axis.

If any of you guys is wondering why I don't use other method, like directly use Robotics Toolbox inverse kinematics functions, I need to create the code directly for an assignment, so far I have been working for a couple of weeks and haven't got any results with inverse kinematics.

[Nathan Hardenberg]

I can't run your first inverse Kinematics. What is "theta(1)" in "Px_w = Px - R*cos(theta(1));"?

In your second inverse Kinematics there are variables that do not get used! But from the points I tried to insert the solution does not really make sense:

[theta1, theta2, theta3, theta4] = inv_kinematics(0.2, 0, 0)
%  0     -1.76   1.76    0     % = resulting solution

As a tip: You can insert points that you already know the solution for and go step by step trough your code, and check if the calculations in between also line up. While this does not guarantee that the solution is correct, it makes it easier to find mistakes.