Initial commit

pend01.m

+l1 = 0.5;  % Pendulum lengths [meter]
+l2 = 0.1;
+fs = 50;   % Plotting rate [Hz]
+h = 1/fs;
+maxt = 4;  % Simulation time [sec]
+g = 9.81;  
+a1 = g/l1;
+a2 = g/l2;
+
+x0 = zeros(12,1);  % Inital state
+x0(1) = pi/180;    % Initial ngles in radians
+x0(2) = -2*pi/180;
+
+% Linearized model
+A = [0  0 1 0 0 0 ; ...
+     0  0 0 1 0 0 ; ...
+     a1 0 0 0 0 0 ; ...
+     0 a2 0 0 0 0 ; ...
+     0  0 0 0 0 1 ; ...
+     0  0 0 0 0 0 ];
+B = [0 ; 0 ; 1/l1 ; 1/l2; 0 ; 1];
+C = [1 0 0 0 0 0 ; ...
+     0 1 0 0 0 0 ; ...
+     0 0 0 0 1 0 ];
+D = [0 ; 0 ; 0];
+ 
+P = ss(A,B,C,D);
+
+% %%%%%%%%% plot open loop bode diagram
+% figure(1)
+% [mag,phase,wout] = bode(P,logspace(-1,2,1000));
+% mag = squeeze(mag);
+% loglog(wout,mag')
+% axis([wout(1) wout(end) 1e-4 1])
+% grid on
+
+
+%%%%%%%%%% design LQG regulator
+Q =  C'*C;
+R = 1e-4;
+[K,S]=lqr(A,B,Q,R);
+
+G = B;
+H = 0*C*B;
+QN = 1;
+RN = diag([1e-3 1e-3 1e-3]);
+syse = ss(A,[B G],C,[D H]);
+[kest,L]=kalman(syse,QN,RN);
+%reg = -lqgreg(kest,K);
+reg = ss(A-L*C-B*K,L,K,0*K*L);
+loopgain = reg*P;
+
+%%%%%%%%%% plot results
+
+figure(2)
+nyquist(loopgain)
+hold on
+nyquist(reg*P,'r')
+axis([-10 10 -10 10])
+
+figure(3)
+nichols(loopgain)
+axis([-500 -100 -40 20])
+grid on
+hold on
+
+figure(4)
+opt = bodeoptions;
+opt.FreqUnits = 'Hz';
+opt.MagUnits = 'abs';
+opt.MagScale = 'log';
+bodemag(loopgain,opt)
+grid on
+hold on
+
+figure(5)
+bodemag(reg,opt)
+grid on
+hold on
+
+% Simulation of closed loop (linearized equations)
+AA = [A -B*K; L*C A-L*C-B*K];
+BB = [eye(size(A)) ; zeros(size(A))];
+CC = [C 0*C; 0*K -K];
+DD = 0*CC*BB;
+systot = ss(AA,BB,CC,DD);
+tvec = 0:h:maxt;
+[y,t,x] = lsim(systot,zeros(length(tvec),6),tvec,x0);
+
+
+figure(6)
+plot(t,180/pi*y(:,1))
+hold on
+plot(t,180/pi*y(:,2),'r')
+xlabel('time [s]')
+ylabel('angles [degrees]')
+
+figure(7)
+plot(t,y(:,3))
+xlabel('time [s]')
+ylabel('position [meter]')
+hold on
+
+figure(8)
+plot(t,y(:,4))
+xlabel('time [s]')
+ylabel('acceleration [m/s^2]')
+hold on
+
+phi1 = y(:,1);
+phi2 = y(:,2);
+pos  = y(:,3);
+u    = y(:,4);
+
+save results
+
+
+

plotit.m

+% -------------------------------------------------------------------
+
+% *****Two Pendula on cart Post-Processing******
+%
+% movie - set to true, the movie will be saved as 'pendulumsoncart.avi'
+% in the root directory
+% 
+% 13/12/2012 - J Whittington - University of Bath
+% 26/01/2018 - Bo Bernhardsson - Lund 
+% 
+% Ref: Patrick Keogh, 2011 - mckplot.m (plotting of mass spring damper)
+% Ref: Alexander Erlich, April 2010 - Animated Double Pendulum
+% (http://www.mathworks.com/matlabcentral/fileexchange/27212)
+%
+% http://engineer.john-whittington.co.uk/2013/04/modelling-a-double-pendulum-in-simulink/
+% 
+% -------------------------------------------------------------------
+
+%Run the model 
+
+pend01
+load results.mat
+
+%--------------Dynamic Plotting--------------
+
+%Create the markers representing the system which will 
+%vary according to calculated positions
+subplot(3,2,[1 3 5])
+
+h1 = plot(0,0,'MarkerSize',30,'Marker','.','LineWidth',2,'Color','b');
+hold on
+h2 = plot(0,0,'MarkerSize',30,'Marker','.','LineWidth',2,'Color','r');
+hold off
+%configure the axis for a suitable animation space
+range=1.1*max(l1,l2); axis([-0.1 0.1 0 range]); 
+title('Pendulum Animation')
+xlabel('x-position [m]'); ylabel('y-position [m]');
+set(gca,'nextplot','replacechildren');  
+
+%Configure dynamic phase plots
+%Angles
+subplot(3,2,2)
+plot(t(1),180/pi*phi1(1),'b.'); 
+hold on
+plot(t(1),180/pi*phi2(1),'r.'); 
+axis([min(t) max(t) -180/pi*max(abs([phi1;phi2])) 180/pi*max(abs([phi1;phi2]))]);  %set axis to min and max values
+xlabel('time') ;ylabel('phi1,phi2 [deg]');
+grid on;
+title('Angles')
+
+%Control signal
+subplot(3,2,4)
+plot(t(1),u(1),'k.');
+hold on
+axis([min(t) max(t) -max(abs(u)) max(abs(u))]);
+xlabel('time') ;ylabel('u [m/s^2]');
+grid on;
+title('Control signal')
+
+%Position
+subplot(3,2,6)
+plot(t(1),pos(1),'k.');
+hold on
+axis([min(t) max(t) -max(abs(pos)) max(abs(pos))]);
+xlabel('time') ;ylabel('position [m]');
+grid on;
+title('Position')
+
+% Animation
+for i=1:length(t)-1
+    if (ishandle(h1)==1) %check figure is plotting   
+        Xcoord=[pos(i),pos(i)-l1*sin(phi1(i))];
+        Ycoord=[0,l1*cos(phi1(i))];
+        %update markers to reflect x and y cords
+        set(h1,'XData',Xcoord,'YData',Ycoord);
+        drawnow;
+    end
+    if (ishandle(h2)==1) %check figure is plotting   
+        Xcoord=[pos(i),pos(i)-l2*sin(phi2(i))];
+        Ycoord=[0,l2*cos(phi2(i))];
+        %update markers to reflect x and y cords
+        set(h2,'XData',Xcoord,'YData',Ycoord);
+    end
+    subplot(3,2,2)
+    plot(t(i),180/pi*phi1(i),'b.')
+    plot(t(i),180/pi*phi2(i),'r.'); 
+    subplot(3,2,4)
+    plot(t(i),u(i),'k.'); 
+    subplot(3,2,6)
+    plot(t(i),pos(i),'k.');
+    drawnow; 
+
+    F(i) = getframe(gcf);
+end
+
+%Save the movie to file 
+
+% vidObj = VideoWriter('pendulumsoncart.mp4','MPEG-4');
+% vidObj.FrameRate = fs;
+% open(vidObj);
+% for i = 1:length(t)-1
+%    writeVideo(vidObj,F(i));
+% end
+% close(vidObj);
+% 
+
+
+
+