作業5-1-2

第二小題

function body(L1,L2,L3,theta1,theta2,theta3)
%設定一個函數可以輸入並繪出手臂位置
%L1為上手臂長,L2為下手臂長,L3為手掌(含手指)長
dA1=10;dA2=6;dB1=6;dB2=4;dC1=4;dfinger=0.5;
t=linspace(pi/2,2.5*pi,20);
Cfinger=dfinger*exp(j*t');
dthumb=0.8;tt=linspace(0,2*pi,20);Cthumb=dthumb*exp(j*tt');
L1x=L1;L1y=0;L2x=L2;L2y=0;
%以上為各個連結點的直徑設定及座標轉換(以linkeshape的形式修改)
th1=-theta1;
th2=theta2-theta1+180;
th3=theta2-theta1+theta3;  
%計算各個水平角度  

CoutA1=max(dA1/2,0.2)*exp(j*t');
CinA1=CoutA1/2;
CoutA2=max(dA2/2,0.2)*exp(j*t');
CinA2=CoutA2/2;   
CoutB1=max(dB1/2,0.2)*exp(j*t');
CinB1=CoutB1/2;
CoutB2=max(dB2/2,0.2)*exp(j*t');
CinB2=CoutB2/2;
CoutC1=max(dC1/2,0.2)*exp(j*t');
CinC1=CoutC1/2;
%轉換成複數座標

%各個連桿之座標設定,並以轉動平移公式計算,繪出圖形
arm1=[0;CinA1;CoutA1(1:10);L1+CoutA2(11:20);
        L1+CinA2;L1+CoutA2(20);CoutA1(1)];
    xx1=real(arm1);
    yy1=imag(arm1);
    x1=xx1*cosd(th1)-yy1*sind(th1);
    y1=xx1*sind(th1)+yy1*cosd(th1);
   
    arm1px=L1x*cosd(th1)-L1y*sind(th1);
    arm1py=L1x*sind(th1)+L1y*cosd(th1);
    line(x1,y1)
    axis equal
   
arm2=[0;CinB1;CoutB1(1:10);L2+CoutB2(11:20);
        L2+CinB2;L2+CoutB2(20);CoutB1(1)];
    xx2=real(arm2);
    yy2=imag(arm2);
    x2=xx2*cosd(th2)-yy2*sind(th2)+arm1px;
    y2=xx2*sind(th2)+yy2*cosd(th2)+arm1py;
  
    arm2px=L2x*cosd(th2)-L2y*sind(th2)+arm1px;
    arm2py=L2x*sind(th2)+L2y*cosd(th2)+arm1py;
    line(x2,y2)
    axis equal
   
arm3=[0;CinC1;CoutC1(1:10);L3/2-3i;
        L3*4/5-2.5i+Cfinger(11:20);L3/2-2.2i+Cfinger(20);
        L3*15/16-i+Cfinger(11:20);L3/2-0.2i+Cfinger(20);
        L3+1.5i+Cfinger(11:20);L3/2+1.7i+Cfinger(20);
        L3*9/10+3.5i+Cfinger(11:20);L3/3+3i+Cfinger(20);
        L3*2/5+7.5i+Cthumb(1:10);L3/4+3i+Cthumb(10);
        CoutC1(1)];
    xx3=real(arm3);
    yy3=imag(arm3);
   
    x3=xx3*cosd(th3)-yy3*sind(th3)+arm2px;
    y3=xx3*sind(th3)+yy3*cosd(th3)+arm2py;
   
    line(x3,y3)
    axis equal