analysis_position_exemple_1.rar_Help!

%function[phi2 , phi3 , phi4 , phi5] = analysis_position_exemple_1(phi1) % les entr�es AB = 0.15; AC = 0.1; CD = 0.15; DF = 0.4; AG = 0.3; phi1= 30* pi/180; wz01 = 50 ; % rd/s xA = 0; yA = 0; xC = 0; % Point A w10 = [0;0;wz10] rA = [xA;yA;0]; % Point B xB = AB * cos(phi1); yB = AB * sin(phi1); rB = [xB;yB;0]; % Point C yC = AC; rC = [xC;yC;0]; % Point D syms solxD solyD eqnD1 = (solxD-xC)^2 + (solyD-yC)^2 == CD^2; eqnD2 = '(solxD-xC)*(yB-yC) == (xB-xC)*(solyD-yC)'; solD = solve(eqnD1 , eqnD2 , solxD , solyD ); xD1 = eval(solD.solxD(1)); yD1 = eval(solD.solyD(1)); xD2 = eval(solD.solxD(2)); yD2 = eval(solD.solyD(2)); if xD1 < xC xD = xD1; yD = yD1; else xD = xD2; yD = yD2; end rD = [xD;yD;0]; % les angles phi2 = pi - atan2(yB-yD,xD-xB) phi3 = phi2 phi4 = pi- atan2(yD-yC,xC-xD) phi5 = phi4 % Point F xF = DF * cos(phi2) + xD; yF = DF * sin(phi2) + yD; syms solxF solyF eqnF1 = '(solxF-xD)^2 + (solyF-yD)^2 == DF^2'; eqnF2 = '(solxF-xD)*(yC-yD) == (xC-xD)*(solyF-yD)'; solF = solve(eqnF1 , eqnF2 , solxF , solyF ); xF1 = eval(solF.solxF(1)); yF1 = eval(solF.solyF(1)); xF2 = eval(solF.solxF(2)); yF2 = eval(solF.solyF(2)); if xF1 > xB xF = xF1; yF = yF1; else xF = xF2; yF = yF2; end rF = [xF;yF;0] % Point G syms solxG solyG eqnG1 = '(solxG-xA)^2 + (solyG-yA)^2 - AG^2'; eqnG2 = '(solxG-xA)*(yD-yA) - (xD-xA)*(solyG-yA)'; solG = solve(eqnG1 , eqnG2 , 'solxG , solyG' ); xG1 = eval(solG.solxG(1)); yG1 = eval(solG.solyG(1)); xG2 = eval(solG.solxG(2)); yG2 = eval(solG.solyG(2)); if xG1 < xA xG = xG1; yG = yG1; else xG = xG2; yG = yG2; end rG = [xG;yG;0] % Graphe plot([xA,xB],[yA,yB],'b-o','lineWidth',2) hold on plot([xB,xC],[yB,yC],'g-o','lineWidth',2) hold on plot([xB,xF],[yB,yF],'r-o','lineWidth',2) hold on plot([xA,xD],[yA,yD],'y-o','lineWidth',2) hold on plot([xC,xD],[yC,yD],'g-o','lineWidth',2) hold on plot([xD,xG],[yD,yG],'k-o','lineWidth',2) hold on fprintf('rA = [ %g, %g, %g ] (m)\n', rA) xlabel('x (m)'), ylabel('y (m)'); axis([-0.35 0.3 -0.3 0.4]); text(xA,yA,'\leftarrow A = ground','HorizontalAlignment','left') text(xC,yC,'\leftarrow C = ground','HorizontalAlignment','left') text(xB,yB+0.005,'B') text(xD,yD+0.005,'D') text(xF,yF+0.005,'F') text(xG,yG+0.005,'G') % %Recherche des inconnus : % syms solwz21 solwz03 % w21 = [0;0;solwz21]; % w03 = [0;0;solwz03]; % vB32 = solvB32 * [cos(phi3) ; sin(phi3); 0 ]; % w05 = [0;0;solwz05]; % w43 = [0;0;solwz43]; % vD54 = solvD54 * [cos(phi5) ; sin(phi5); 0 ]; % % eqn1 = wz10 + solwz21 + solwz03 == 0 ; % eqn2 = vB32 + cross(rA,w01) + cross(rB,w21) + cross(rC,w03) == 0 ; % eqn2x = eqn2(1); % eqn2y = eqn2(2); % eqn3 = - solwz03 + solwz43 + solwz05 == 0 ; % eqn4 = vD54 + cross(rA,w05) + cross(rC,-w03) + cross(rD,w43); % eqn4x = eqn4(1); % eqn4y = eqn4(2); % % solu = solve(eqn1,eqn2x,eqn2y,eqn3,eqn4x,eqn4y, solwz21,solwz03,vB32,solwz43,solwz05,solvD54); % % wz21 = % wz03 = A = [ 1 1 0 0 0 ; 0 -1 1 1 0 0 ; yB yC 0 0 cos(phi3) 0 ; xB xC 0 0 -sin(phi3) 0 ; 0 -yC yD yA 0 cos(phi5) ; 0 -xC xD xA 0 -sin(phi5)];