I was thinking of someway to add the link length a2 to the calculations. Figured out that I could just shift the link2 coordinate frame back to joint 2. This is the update in the diagram.

And I also noticed the joint values being numbered wrong. Did that correction.

Now I feel satisfied. Next is Inverse Kinematics!