function [] = Gantry2Display(X, Xe, Ref) % Double Pendulum system - display routine % ECE 463 Test #2 % cart = 1kg % m1 = m2 = 1kg % L1 = L2 = 1m % X = [x, q1, q2, dx, dq1, dq2] % Observer x1 = Xe(1); y1 = 0; q1 = Xe(2); q2 = Xe(3); % cart xc = [-0.2,0.2,0.2,-0.2,-0.2] + x1; yc = [0,0,0.2,0.2,0]; x2 = x1 + sin(q1); y2 = y1 - cos(q1); x3 = x2 + sin(q1+q2); y3 = y2 - cos(q1+q2); % ball q = [0:0.1:1]' * 2*pi; xb = 0.05*cos(q); yb = 0.05*sin(q); hold off plot(xc, yc,'g',[x1,x2,x3],[y1,y2,y3],'g',xb+x2, yb+y2, 'g',xb+x3, yb+y3, 'g-') ylim([-2.5,0.5]); hold on x1 = X(1); y1 = 0; q1 = X(2); q2 = X(3); % cart xc = [-0.2,0.2,0.2,-0.2,-0.2] + x1; yc = [0,0,0.2,0.2,0]; x2 = x1 + sin(q1); y2 = y1 - cos(q1); x3 = x2 + sin(q1+q2); y3 = y2 - cos(q1+q2); % ball q = [0:0.1:1]' * 2*pi; xb = 0.05*cos(q); yb = 0.05*sin(q); plot([-3,3],[0,0],'b',xc, yc,'r',[x1,x2,x3],[y1,y2,y3],'r',xb+x2, yb+y2, 'r',xb+x3, yb+y3, 'r-',[Ref, Ref],[-0.1,0.1],'b') ylim([-2.5,0.5]); pause(0.01); end