function cartesian_coords = forward_kinematics(thetas)

    theta1 = thetas(1);
    theta2 = thetas(2);
    theta3 = thetas(3);
    theta4 = thetas(4);
    theta5 = thetas(5);

    % Transformation matrices
    A1 = create_roation_matrix(0,90,10.5,theta1);
    A2 = create_roation_matrix(13.5, 0, 0, theta2);
    A3 = create_roation_matrix(16, 0, 0, theta3);
    A4 = create_roation_matrix(5.5, -90, 0,-(theta2+theta3));
    A5 = create_roation_matrix(0, 0, -7, theta5);

    % Multiply them all together
    homogenous_transformation_matrix = A1*A2*A3*A4*A5

    % Get x,y,z
    x = homogenous_transformation_matrix(1,4);
    y = homogenous_transformation_matrix(2,4);
    z = homogenous_transformation_matrix (3,4);

    cartesian_coords = [x,y,z];

end

% Create transformation from dh parameters
% alpha and theta in degrees
% r and d in cms
function A = create_roation_matrix(r, alpha, d, theta)
    A = [cos(theta), -sin(theta)*cos(alpha), sin(theta)*sin(alpha), r*cos(theta);
        sin(theta), cos(theta)*cos(alpha), -cos(theta)*sin(alpha), r*sin(theta);
        0, sin(alpha), cos(alpha), d;
        0, 0, 0, 1];
end