clear

%%%%%%%%%%%%%%%%%%% CINEMATICA DIRECTA %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

d1=3 % d1=input ('introducir longitud de la base

rotacional(eslabon1)=')
l2=3*sqrt(2) % l1=input ('introducir longitud del brazo
rotacional(eslabon2)=')
l3=2*sqrt(5) % l2=input ('introducir longitud del antebrazo
rotacional(eslabon3)=')
l4=1 % l3=input ('introducir longitud de la mu�eca
rotacional(eslabon4)=')

px=input ('posicion final en x')

py=input ('posicion final en y')
pz=input ('posicion final en z')

q6=input('ingrsar numero de vueltas en la pinza la pinza');

q5=input('ingrsar angulo de sujeccion en la pinza');

theta5=q5*pi/180
theta6=q6*pi/180

L{1} = link([ pi/2 0 0 d1 0 ],'standard');

L{2} = link([ 0 l2 pi/4 0 0 ],'standard');
L{3} = link([ 0 l3 -1.2491 0 0 ],'standard');
L{4} = link([ pi/2 0 -1.1071 0 0 ],'standard');
L{5} = link([ 0 0 0 l4 0 ], 'standard');

robot_fer=robot(L);

%calculo de theta1

theta1=atan(py/px)

q1=theta1*180/pi

%claculo de theta3

a=l4*sin(theta5)
b=l4*cos(theta5)

x=px-a*cos(theta1)
y=py-a*sin(theta1)

c=pz+b-d1

xy=sqrt(x^2+y^2)

beta=atan(c/xy)

h=sqrt(xy^2+c^2)

m=(h^2-l2^2-l3^2)/(2*l2*l3)

theta3=-1*acos(m)
f=abs(theta3)
%calculo de theta2

alfa=atan(l3*sin(f)/(l2+l3*cos(f)))

theta2=alfa+beta

%calculo de theta4

theta7=theta2+theta3

theta4=theta5-theta7-pi/2

q1=theta1*180/pi
q2=theta2*180/pi
q3=theta3*180/pi
q4=theta4*180/pi
q6=theta6*180/pi

s1=q1
s2=q2-45;
s3=q3+71.57;
s4=q4+63.43;
s6=q6

t1=s1*pi/180;
t2=s2*pi/180;
t3=s3*pi/180;
t4=s4*pi/180;
t6=s6*pi/180;

A01=MDH (t1,d1,0,pi/2);
A12=MDH (t2+pi/4,0,l2,0);
A23=MDH (t3-1.2491,0,l3,0);
A34=MDH (t4+0.4636,0,0,pi/2);
A45=MDH (t6,l4,0,0);

T05=A01*A12*A23*A34*A45

x=T05(1,4)
y=T05(2,4)
z=T05(3,4)

qq=ikine(robot_fer,T05);

plot(robot_fer,qq)