% Double Pendulum system - main routine % cart = 1kg % m1 = m2 = 1kg % L1 = L2 = 1m % X = [x, q1, q2, dx, dq1, dq2] % homework #6: pole placement X = [0, 0, 0, 0, 0, 0]'; Ref = 1; dt = 0.01; U = 0; t = 0; Kx = [10 0 0 10 0 0]; Kr = 10; y = []; n = 0; while(t < 15) Ref = 1; U = Kr*Ref - Kx*X; dX = Gantry2Dynamics(X, U); X = X + dX * dt; t = t + dt; n = mod(n + 1, 5); if(n == 0) Gantry2Display(X, Ref); plot([Ref, Ref],[-0.1,0.1],'b'); y = [y; X(1)]; end end t = [1:length(y)]' * dt; plot(t,y);