g=9.81; 
r=.003; %3mm
l=.025; %25mm
m=0.05; %50g

theta = [0:pi/720:pi/2];
x = r*(2*cos(theta)-cos(pi/2-2*theta));
subplot(3,2,1), plot(2*theta*180/pi,x);
xlabel('angle of joint (deg)');
ylabel('xposition of muscle contact point');

y = r*(2*sin(theta)+sin(pi/2-2*theta))-r;
%figure
subplot(3,2,2), plot(2*theta*180/pi,y);
xlabel('angle of joint (deg)');
ylabel('yposition of muscle contact point');

%figure
subplot(3,2,3), plot(x,y);
ylabel('y trajectory');
xlabel('x trajectory');
axis equal;


Phi = atan(y./x);
subplot(3,2,4), plot(2*theta*180/pi,Phi);
xlabel('angle of joint (deg)');
ylabel('Phi');

distance = sqrt(x.^2 + y.^2);
subplot(3,2,5), plot(2*theta*180/pi,distance);
xlabel('angle of joint (deg)');
ylabel('distance');

ShortTheta = [0:pi/720:pi/4];
beta = (pi/2 + theta)./2;
alpha = pi/2 + Phi - theta + beta;
%figure
%plot(theta, alpha)
delta = 2*r*sin((pi/2-theta)/2);
den = delta.*cos(alpha);

F = (m*g*l*cos(2*theta(1:180)))./(den(1:180));
subplot(3,2,6), plot(2*180/pi*theta(1:180),F);
axis([0,100,-10,2]);
xlabel('angle of joint (deg)');
ylabel('Artificial Muscle Force (N)');
grid on;