l2=input('Ingrese el valor de largo2: ')

l3=input('Ingrese el valor de largo3: ')

xi=0
yi=l2+l3
if l2>l3
re=l2+l3
ri=l2-l3
else
re=l2+l3
ri=l3-l2
end
xf=input('Ingrese el valor de xf: ')
yf=input('Ingrese el valor de yf: ')
r= sqrt(xf^2+yf^2)
if r>re
disp('El valor exede el rango del robot: ')
else
if r<ri
disp('El valor es menor del alcanse del robot: ')
else
zi=3
zf=3
z=3
l1=3
xp=(xf)/6
x1=xp
x2=(xp*2)
x3=(xp*3)
x4=(xp*4)
x5=(xp*5)
y1=(yf-yi)*((x1-xi)/(xf-xi))+yi
y2=(yf-yi)*((x2-xi)/(xf-xi))+yi
y3=(yf-yi)*((x3-xi)/(xf-xi))+yi
y4=(yf-yi)*((x4-xi)/(xf-xi))+yi
y5=(yf-yi)*((x5-xi)/(xf-xi))+yi
preq1=(x1^2+y1^2+z^2-l2^2-l3^2)/(2*l2*l3)
preq2=(x2^2+y2^2+z^2-l2^2-l3^2)/(2*l2*l3)
preq3=(x3^2+y3^2+z^2-l2^2-l3^2)/(2*l2*l3)
preq4=(x4^2+y4^2+z^2-l2^2-l3^2)/(2*l2*l3)
preq5=(x5^2+y5^2+z^2-l2^2-l3^2)/(2*l2*l3)
preq6=(xf^2+yf^2+z^2-l2^2-l3^2)/(2*l2*l3)
q1=acosd(real(preq1))
q2=acosd(real(preq2))
q3=acosd(real(preq3))
q4=acosd(real(preq4))
q5=acosd(real(preq5))
q6=acosd(real(preq6))
z32=(atand(3/(+sqrt
))
z33=(atand(3/(+sqrt
))
z34=(atand(3/(+sqrt
))
z35=(atand(3/(+sqrt
))

((x1)^2+y1^2))))-(atand((l3*sind(q1))/((l2)+(l3)*(cosd(q1)))
((x2)^2+y2^2))))-(atand((l3*sind(q2))/((l2)+(l3)*(cosd(q2)))
((x3)^2+y3^2))))-(atand((l3*sind(q3))/((l2)+(l3)*(cosd(q3)))
((x4)^2+y4^2))))-(atand((l3*sind(q4))/((l2)+(l3)*(cosd(q4)))

z36=(atand(3/(+sqrt ((x5)^2+y5^2))))-(atand((l3*sind(q5))/((l2)+(l3)*(cosd(q5)))
))
z37=(atand(3/(+sqrt ((xf)^2+yf^2))))-(atand((l3*sind(q6))/((l2)+(l3)*(cosd(q6)))
))
qy1=real(z32)
qy2=real(z33)
qy3=real(z34)
qy4=real(z35)
qy5=real(z36)
qy6=real(z37)

x = 1:6;
x2 = 1:6;
y = [q1 q2 q3 q4 q5 q6];
y2 = [qy1 qy2 qy3 qy4 qy5 qy6];
xx = 1:0.1:6;
xx2 = 1:0.1:6;
yy = spline(x,y,xx);
yy2 = spline(x2,y2,xx2);
plot(x,y,'o',xx,yy,x2,y2,'o',xx2,yy2)
end
end