% date file open

fid = fopen('twolink.dat', 'w');

% input two link robot arm length

link1 = input('Enter length of link1:');

link2 = input('Enter length of link2:');

length1 = link1;

length2 = link2;

% input t_end

t_end = input('Enter t_end:');

% input 가시화 option

op1 = input('Enter path_on/off[1/0]:');

op2 = input('Enter velocity_on/off[1/0]:');

op3 = input('Enter no of images_on/off[1/0]:');

%input n_frame

n_frame  = input('Enter no of images:');

% define local vectors r1, r2

r1_prime = [length1; 0];

r2_prime = [length2; 0];

% define simulation time parameters

t_start = 0.0;

t_step = t_end*0.01;

% initialize current time t for loop

t = t_start;

while (t < t_end+t_step)

    % define angle direction of link, theta_dot, theta_ddot

    [theta_1, theta_1dot, theta_1ddot] = driver_1(t);  

    [theta_2, theta_2dot, theta_2ddot] = driver_2(t);    

    % transform local vector to global vectors

    vector1 = trans(r1_prime, theta_1);

    vector2 = trans(r2_prime, theta_2);

    % compute end effector postion of two link robot

    rP = addition(vector1, vector2);

    % velocity

    r1_dot = velocity(theta_1, theta_1dot, r1_prime);

    r2_dot = velocity(theta_2, theta_2dot, r2_prime);

    rP_dot = addition(r1_dot, r2_dot);

    % acceleration

    r1_ddot = acceleration(theta_1, theta_1dot, theta_1ddot, r1_prime);

    r2_ddot = acceleration(theta_2, theta_2dot, theta_2ddot, r2_prime);

    rP_ddot = addition(r1_ddot, r2_ddot);

    % save end effector postion data into the data file twolink.dat

    fprintf(fid,'%7.5f %15.6e %15.6e %15.6e %15.6e %15.6e %15.6e %15.6e %15.6e\n', t,rP(1), rP(2),  rP_dot(1),  rP_dot(2), rP_ddot(1), rP_ddot(2), vector1(1), vector1(2));

    t = t + t_step;

end

fclose(fid);

%% postprocess

% load saved data file for plot

load twolink.dat

% load saved data file to define time, x_p, y_p for plot

time = twolink(:,1);

x_p= twolink(:,2);

y_p= twolink(:,3);

x_pdot= twolink(:,4);

y_pdot= twolink(:,5);

x_pddot= twolink(:,6);

y_pddot= twolink(:,7);

L1_x= twolink(:,8);

L1_y= twolink(:,9);

%% x_p, y_p plot setting

% x_p setting

figure(1)

subplot(1,2,1);

set(gcf,'color',[1,1,1]);

plot(time, x_p,'r', time, x_pdot,'-b', time, x_pddot,'--g');

xlabel('Time(s)','FontSize',14);

ylabel('x_p','FontSize',14);

title('end-effector x','FontSize',14);

legend('x_p', 'x_pdot', 'x_pddot');

% y_p setting

subplot(1,2,2);

set(gcf,'color',[1,1,1]);

plot(time, y_p,'r', time, y_pdot,'-b', time, y_pddot,'--g');

xlabel('Time(s)','FontSize',14);

ylabel(y_p','FontSize',14);

title('end-effector y','FontSize',14);

legend('y_p', 'y_pdot', 'y_pddot');

%% for video

% video file 이름 만들기

A = name(t_end);

B = name(op1);

C = name(n_frame);

vfn = ['ani-A-B-C'];

% video file open

v = VideoWriter(vfn);

open(v);

figure(2);

% 그라운드 만들기

rectangle('Position', [-2 -1 4 1], 'facecolor', 'g');

for i = 1:length(time)

% 잔상 control

    if (op3 == 1)

        n2 = fix((length(time))/n_frame);

    else

        n_frame = 0;

    end

    hold on;

    % Link1, Link2 만들기

    Link1 = line([0 L1_x(i)], [0 L1_y(i)], 'color', 'k', 'Linewidth', 2);

    Link2 = line([L1_x(i) x_p(i)], [L1_y(i) y_p(i)], 'color', 'k', 'Linewidth', 2); 

    % velocity vector 만들기

    vel = quiver(x_p(i), y_p(i), x_pdot(i), y_pdot(i), 'Color', 'g', 'Linewidth', 2);

    % velocity vector 가시화 control

    if (op2 ==0)

        delete(vel);

    end

    % end effector 궤적 만들기

    path = plot(x_p(i), y_p(i), 'r', 'MarkerSize', 2);

    % end effector 궤적 control

    if (op1 == 0)

        delete(path);

    end

    % axis로 그래프 setting

    axis equal;

    axis([-7 7 -7 7]);

    % 각관절에 원 만들어 넣기

    r=0.3;

    [x1, y1] = circle(0,0,r);

    [x2, y2] = circle(L1_x(i), L1_y(i), r);

    [x3, y3] = circle(x_p(i), y_p(i), r);

    % 원 색깔 채우기

    cir1 = fill(x1, y1, 'w');

    cir2 = fill(x2, y2, 'r');

    cir3 = fill(x3, y3, 'r');

    F(i) = getframe;

    if(i==length(time)) 

        break;

    end

             if(0==rem(i, n2))

             else

                  delete(Link1);

                  delete(Link2);

                  delete(vel);

             end

    delete(cir1);

    delete(cir2);

    delete(cir3);

end

writeVideo(v, F);

close(v);


대체 왜자꾸 원안에 색이 반만칠해지고 궤도도 안그려지고 도대체가 왜그런건지 모르겠다..