% Double Pendulum system - main routine % ECE 463 Lecture #10 % cart = 1kg % m1 = m2 = 1kg % L1 = L2 = 1m % X = [x, q1, q2, dx, dq1, dq2] X = [0, 0.2, 0.2, 0, 0, 0]'; Ref = 1; dt = 0.01; U = 0; t = 0; Kx = [10 0 0 10 0 0]; Kr = 10; n = 0; while(t < 20) Ref = sign(sin(0.1*pi*t)); 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'); end end