function [x,y] = ang2xy(th1,th2)

thoff = 0; % pi/2;  % Offset is angle of th1 when encoder is initialized
th1off = th1 - thoff;
L1 = 178;  % mm
L2 = 88; % mm

x = L1*cos(th1off) + L2*cos(th1off+th2);
y = L1*sin(th1off) + L2*sin(th1off+th2);



