matlab para cuadricula articulada

3
clc,clear%ejemplo planteado en la página 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 variación 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; también puede ser a\b que será lo mismo vt3=x(1); esto es la variación de teta 3, es decir el error y si no responde se extrae la información 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 estén permitidos en el error de lo contrario seguirá hasta k se encuentre vt4=x(2);

Upload: endry-rodriguez-paredes

Post on 09-Dec-2015

10 views

Category:

Documents


1 download

DESCRIPTION

.

TRANSCRIPT

Page 1: matlab para cuadricula articulada

clc,clear%ejemplo planteado en la página 413 mabieclearAM=[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 AQUIchi=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.7210for i=1:73 for parte desde aquí hasta el final con una variación 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; también puede ser a\b que será lo mismo vt3=x(1); esto es la variación de teta 3, es decir el error y si no responde se extrae la información 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 estén 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

Page 2: matlab para cuadricula articulada

%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 T4 w3 w4 alfa3 alfa4 VCC RCC ACC rcx rcy')disp (AM)T2=AM(:,1);T3=AM(:,2);T4=AM(:,3);w3=AM(:,4);W4=AM(:,5);alfa3=AM(:,6);

Page 3: matlab para cuadricula articulada

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