Updated to use geometric method to calculate first (shoulder_pan) motor angle.
Updated to use geometric method to calculate first (shoulder_pan) motor angle.

MRSDTeamI/bud-e,MRSDTeamI/bud-e,MRSDTeamI/bud-e,MRSDTeamI/bud-e,MRSDTeamI/bud-e,MRSDTeamI/bud-e