%% spec

clear

clc

L1 = 1; L2 = 1; r=0.5; % 링크 1 , 2 의 길이 및 경로 원의 반지름

phi= 0:1:360; % 0~ 360 까지 1 씩 값을 올림

xx = 1 + r*cosd(phi); % (1.1)에 r = 0.5 인 원을 그리기 위한 x 값

X = [xx xx xx xx xx]; %matrix를 만듬 phi가 0~360까지

yy = 1 + r*sind(phi); % (1.1)에 r = 0.5 인 원을 그리기 위한 y 값

Y = [yy yy yy yy yy]; %matrix를 만듬 phi가 0~360까지

%% plot

for i = 1:length(X) % X 행렬까지 반복 1부터 360까지의 X좌표 Y좌표를 하나하나 inverse kinematics 를 하기위함

x = X(i);

y = Y(i);

theta2 = acosd((x^2 + y^2 - L1^2 - L2^2)/(2*L1*L2)); % 세타2 값 정의

theta1 = asind((L2*sind(theta2))/(sqrt(x^2 + y^2))) + atand(y/x); % 세타1 값 정의

x1 = L1*cosd(theta1); % link 1 의 경로 x좌표

y1 = L1*sind(theta1); % 경로 y좌표

figure(1)

plot(X,Y,'k') % 원을 그림 'k'는 검은색을 나타냄

grid on %

hold on % 고정

H=plot([0 x1],[0 y1],'r','Linewidth',5) % 링크 1을 그린다?

hold on

H1=plot([x1 x],[y1 y],'b','Linewidth',5) % 링크 2을 그린다?

axis([-2 2 -2 2])

pause(0.01) % 빨리 돌게 하고싶으면 이거 시간 작게하면됩니당

delete(H) % delete가 plot을 iter마다 지워줌 근데 원은지우면안되니까 링크만지웁니다.

delete(H1)

end