clc,clear%ejemplo planteado en la pgina 413 mabie

clear
AM=[0,0,0,0,0,0,0,0,0,0,0,0];ACUMULA INFORMACION O MATRIZ DE ACUMULACION
LO QUE HAGO EN AM ABAJO SI AGREGO ALGO MAS DEBO AUMENTAR UN CERO MAS AQUI
chi=0;
a=1.0; %longitud del eslabon 2
b=2.06; %longitud del eslabon 3
c=2.33; %longitud del eslabon 4
d=2.22; %longitud del eslabon 1
%el angulo t1 es cero y aparece en F1
%si desea considere t1 en su programa porque no siempre es cero
r5=2.6229;
r6=-1.5760;
t1=0*pi/180;%empezamos de cero para que el programa corra desde cero
t2=0*pi/180; %66.7705
t3=45.*pi/180;
t4=100.*pi/180;%arbitrario
w2=1;%0.2561
alfa2=-100;%0.7210
for i=1:73 for parte desde aqu hasta el final con una variacin de
360/73=5 es decir cada 5 grados como lo que hicmos e la tarea.
f1=a.*cos(t2)+b.*cos(t3)-c.*cos(t4)-d.*cos(t1);son valores que se
sacan de la cuadricula artuculada es decir en lo de los lazos
f2=a.*sin(t2)+b.*sin(t3)-c.*sin(t4)-d.*sin(t1);
A=[-b*sin(t3) c.*sin(t4);b.*cos(t3) -c.*cos(t4)];se pones ; para que
no qparesca en pantalla el desarrollo
B=-[a.*cos(t2)+b.*cos(t3)-c.*cos(t4)d.*cos(t1);a.*sin(t2)+b.*sin(t3)-c.*sin(t4)-d.*sin(t1)];
x=inv(A)*B; tambin puede ser a\b que ser lo mismo
vt3=x(1); esto es la variacin de teta 3, es decir el error y si no
responde se extrae la informacin para seguir trabajando
vt4=x(2);
e=0.00001;
while abs(vt3)>e & abs(vt4)>ewile maso me dice que mietras lo que se
sentencia tncs se realizara lo que sigue
t3=t3+vt3;
t4=t4+vt4;
f1=a.*cos(t2)+b.*cos(t3)-c.*cos(t4)-d.*cos(t1);
f2=a.*sin(t2)+b.*sin(t3)-c.*sin(t4)-d.*sin(t1);
A=[-b*sin(t3) c.*sin(t4);b.*cos(t3) -c.*cos(t4)];
B=-[a.*cos(t2)+b.*cos(t3)-c.*cos(t4)-d.*cos(t1);
a.*sin(t2)+b.*sin(t3)-c.*sin(t4)-d.*sin(t1)];
x=inv(A)*B;
vt3=x(1);se hara lo de wile hasta que estos dos valores estn
permitidos en el error de lo contrario seguir hasta k se encuentre
vt4=x(2);
%definimos velocidades w3, w4en velocidades ya no se usa newton
raphson solo se usa las ecuaciones
%f3=-a*w2.*sin(t2)-b*w3*sin(t3)+c*w4*sin(t4)=0;
%f4=a*w2.*cos(t2)+b*w3*cos(t3)-c*w4*cos(t4)=0;
Jv=[-b*sin(t3) c.*sin(t4);b.*cos(t3) -c.*cos(t4)];
C=[a*w2*sin(t2);-a*w2*cos(t2)];
Y=inv(Jv)*C;
w3=Y(1);
w4=Y(2);
%definimos aceleraciones alfa3 y alfa4

%f5=-a*alfa2*sin(t2)-a*w2^2*cos(t2)-b*alfa3*sin(t3)b*w3^2*cos(t3)+c*alfa4*sin(t4)+c*w4^2*cos(t4)=0;
%f6=a*alfa2*cos(t2)-a*w2^2*sin(t2)+b*alfa3*cos(t3)b*w3^2*sin(t3)-c*alfa4*cos(t4)+c*w4^2*sin(t4)=0;
Ja=[-b*sin(t3) c.*sin(t4);b.*cos(t3) -c.*cos(t4)];
D=[a*alfa2.*sin(t2)+a*w2^2.*cos(t2)+b*w3.^2*cos(t3)c*w4.^2*cos(t4);
-a*alfa2.*cos(t2)+a*w2^2.*sin(t2)+b*w3.^2*sin(t3)c*w4.^2*sin(t4)];
Z=inv(Ja)*D;
alfa3=Z(1);
alfa4=Z(2);
end
%TRAYECTORIA DE C
rcx=a*cos(t2)+r5*cos(t3)+r6*cos(t3+pi/2);
rcy=a*sin(t2)+r5*sin(t3)+r6*sin(t3+pi/2);
RCC=sqrt(rcx^2+rcy^2);

%VELOCIDAD DE C
vcx=-a*w2*sin(t2)-r5*w3*sin(t3)-r6*w3*cos(t3);
vcy=a*w2*cos(t2)+r5*w3*cos(t3)-r6*w3*sin(t3);
VCC=sqrt(vcx^2+vcy^2);
%ACELERACIONES DE C
acx=-a*alfa2*sin(t2)-a*w2*w2*cos(t2)-(r5*alfa3-r6*w3*w3)*sin(t3)(r6*alfa3+r5*w3*w3)*cos(t3);
acy=a*alfa2*cos(t2)-a*w2*w2*sin(t2)+(r5*alfa3-r6*w3*w3)*cos(t3)(r6*alfa3+r5*w3*w3)*sin(t3);
ACC=sqrt(acx^2+acy^2);%total
M1=[t2*180/pi,t3*180/pi,t4*180/pi,w3,w4,alfa3,alfa4,VCC,RCC,ACC,rcx,rcy];
ANTES DE ESTO PUDES PONER TDO, ESTO E SIMPORTANTE,

if chi==0
AM=M1;
chi=1;
else
AM=[AM;M1];
end
t2=t2+5*pi/180;
end
disp (' T2 T3
rcy')
disp (AM)
T2=AM(:,1);
T3=AM(:,2);
T4=AM(:,3);
w3=AM(:,4);

T4

w3

w4

alfa3

alfa4

VCC

RCC

ACC

rcx

W4=AM(:,5);
alfa3=AM(:,6);
alaf4=AM(:,7);
VCC=AM(:,8);
RCC=AM(:,9);
ACC=AM(:,10);
rcx=AM(:,11);
rcy=AM(:,12);
figure (1)
plot(T2,RCC)
grid
figure (2)
plot(T2,VCC)
grid
figure (3)
plot(T2,ACC)
grid
figure (4)
plot(rcx,rcy)
grid