% Samuel Barnes  5 March 04
% Control System and Simulation for the twin-pendulum system.
clear all;

%User Set Parameters
freq   = 115;                    %Hertz      Sample Freq
T      = 1/freq;                 %Seconds    Sample Interval
theta  = 10*pi/180;              %radians    Spacing of poles in S plane
Wn     = 11;                      %magnitude  Polar magnitude of poles in S plane

WnL    = 2*Wn;                   % radius    Observer Pole Locations
thetaL = 10*pi/180;              % angle

g = 9.81;           % m/s^2     Gravitational constant

% SYSTEM PARAMETERS
% Measured Mechanical Parameters
d1 = 0.32+0.003;	% m			Length of pendulum 1 (long)
d2 = 0.079;         % m			Length of pendulum 2 (short)
mp1 = 0.0208;	    % kg		Mass of pendulum 1
mp2 = 0.0050;	    % kg		Mass of pendulum 2
m = 0.391;	        % kg		Mass of carriage
rd = 0.0254/2;      % m			Drive pulley radius
md = 0.0375; 	    % kg		Mass of drive pulley (cylinder)
mc1 = 0.0036;	    % kg		Mass of clamp 1*
mc2 = 0.0036;	    % kg		Mass of clamp 2*

% *Clamp Dimensions
%  Rectangular 0.0254 x 0.01143 m
%  The pivot shaft is 0.00714 m from the end

% Motor Parameters (Data Sheet)
Im = 43e-7;     % kg m^2/rad	Rotor moment of inertia
R = 4.09;   	% ohms		    Resistance
kt = 0.0351;    % Nm/A		    Torque constant
ke = 0.0351;    % Vs/rad		Back emf constant
av = 1;          % V/V           Motor Voltage Gain

% Derived Mechanical Parameters
Lc1 = .0254;                    % m             Length Clamp 1
Wc1 = .01143;                   % m             Width Clamp 1
Dc1 = .00714;                   % m             Distance to Pivot Clamp 1
Ic1 = 1/9*mc1*(Lc1^2+1/4*Wc1^2);% kg m^2/rad	Moment of inertia, clamp 1  
Ic2 = Ic1;                      % kg m^2/rad	Moment of inertia, clamp 2
Id = 1/2*md*rd^2;               % kg m^2/rad	Moment of inertia, drive pulley
Imd = Im + Id;                  % kg m^2/rad    Moment of inertia, combined

J1 = Ic1 + mp1*(d1^2)/3;        % Total moment of inertia, pendulum 1 (long)
J2 = Ic2 + mp2*(d2^2)/3;        % Total moment of inertia, pendulum 2 (short)
Jd = Im + Id;                   % Total moment of inertia, motor drive
Mc = m + mc1 + mc2;             % Total carriage mass (including clamps)

% Friction Test Data
%   Carriage Slope = 19 deg;  Terminal Velocity xdotss = 0.312 m/s; From
%        twincarriage.m;
%   Pendulum 1 (long) Exponent a1 = 0.0756 1/s;  From longfit.m
%   Pendulum 2 (short) Exponent a2 = 0.2922 1/s; From shortfit.m

%alpha = 19*pi/180;
%xdotss = 0.312;
%a1 = 0.0756;
%a2 = 0.2922;
alpha = 10.1*pi/180;
xdotss = 0.108;
a1 = 0.0756;
a2 = 0.2193;

                                % Ns/m	    Viscous friction of carriage system
b = (m+mp1+mp2+mc1+mc2)*g*sin(alpha)/xdotss;
b1 = 2*J1*a1;    	    % Nms/rad	Viscous friction of pendulum 1 (rotational)
b2 = 2*J2*a2;    	    % Nms/rad	Viscous friction of pendulum 2 (rotational)





% Mechanical Equations
M =[1      0       0                   0                       0                   0
    0      1       0                   0                       0                   0
    0      0       1                   0                       0                   0
    0      0       0       mp1+mp2+mc1+mc2+m+Imd/rd^2      mp1*d1/2            mp2*d2/2
    0      0       0              d1*mp1/2                     J1                  0
    0      0       0              d2*mp2/2                     0                   J2];
    

V =[0       0           0           -1                   0    0
    0       0           0            0                  -1    0
    0       0           0            0                   0   -1
    0       0           0           kt*ke/(R*rd^2)+b     0    0
    0   -d1*mp1*g/2      0           0                   b1   0
    0       0       -d2*mp2*g/2      0                   0    b2];

E =[0
    0
    0
    kt*av/(R*rd)
    0
    0];

% Calculation of state matrices
F = -M\V;
G =  M\E;
H = [1 0 0 0 0 0 
     0 1 0 0 0 0
     0 0 1 0 0 0];
J = [0
     0
     0];
 
 [m n] = size(H);
 
 sysC = ss(F,G,H,J);
 sysD = c2d(sysC,T,'zoh');              %convert system to discrete
 [phi, gamma, C, D,] = ssdata(sysD);
 
 
%Calculate Control Law 
p1c = Wn*exp(j*(pi-1/2*theta));         %place poles in s plane
p2c = conj(p1c);
p3c = Wn*exp(j*(pi-3/2*theta));
p4c = conj(p3c);
p5c = Wn*exp(j*(pi-5/2*theta));
p6c = conj(p5c);
                                        %convert poles to z plane
pd  = [exp(T*p1c) exp(T*p2c) exp(T*p3c) exp(T*p4c) exp(T*p5c) exp(T*p6c)]';

K   = acker(phi, gamma,pd);
Cr =[1 0 0 0 0 0];                      % Output to be controlled
Dr =[0];
                                        %Calculate Nbar
Nxu = [phi-eye(size(phi))    gamma
        Cr                    Dr]^-1*[zeros(n,1);1];
Nx = Nxu(1:n);
Nu = Nxu(n+1);
Nb = Nu + K*Nx;                         
                                        %Calculating Lp
p1L = WnL*exp(j*(pi-1/2*thetaL));       
p2L = conj(p1L);
p3L = WnL*exp(j*(pi-3/2*thetaL));
p4L = conj(p3L);
p5L = WnL*exp(j*(pi-5/2*thetaL));
p6L = conj(p5L);

pL = [exp(T*p1L) exp(T*p2L) exp(T*p3L) exp(T*p4L) exp(T*p5L) exp(T*p6L)]';

Lp = place(phi', H', pL)';


%Reduced Order Observer (Used)
phiaa = phi(1:m, 1:m);
phiab = phi(1:m, (m+1):n);
phiba = phi((m+1):n, 1:m);
phibb = phi((m+1):n, (m+1):n);

gammaa = gamma(1:m);
gammab = gamma((m+1):n);

Ha = H(:, 1:m);
Hb = H(:, (m+1):n);

Ka = K(:, 1:m);
Kb = K(:, (m+1):n);

p0L = -WnL;
%p0L = -25;
%p1L = -29;
%p2L = -36;
pLr = [exp(T*p1L) exp(T*p2L) exp(T*p0L) ]';

Lpr = place(phibb', (Ha*phiab)', pLr)';

coefw = (phibb-Lpr*Ha*phiab);
coefu = (gammab-Lpr*Ha*gammaa);
coefy = ((phibb-Lpr*Ha*phiab)*Lpr+phiba*inv(Ha)-Lpr*Ha*phiaa*inv(Ha));





%Actual System

%**Initialize**
ang1Lim = 15*pi/180;        %degrees
ang2Lim = 30*pi/180;        %degrees
vLim    = 24;               %Voltage
timeLim = 60;               %seconds

intervalLim = timeLim*freq;
datay = zeros(3,intervalLim);
datau = zeros(1,intervalLim);
dataw = zeros(3,intervalLim);
%y1 = length(size(intervalLim));
%y2 = length(size(intervalLim));
%y3 = length(size(invervalLim));

%conmat = (phi-Lp*H);
%xbar = [0 0 0 0 0 0]';
%xbar = (conmat)*xbar+gamma*u+Lp*y;

r = 0;                                              
y = [0 0 0]';                                        %Set initial Values
u = 0;
w = [0 0 0]';

%Reduced Order Observer Parameters
w = (coefw)*w+(coefu)*u+(coefy)*y;
xa = y;
xbarb = w+Lpr*y;
u = -Ka*xa-Kb*xbarb+Nb*r;

disp('Initializing Sys');                        %Intialization Mesa Commands
MesaClean(1);
MesaInit([0 1 2 3], freq);
MesaClearEnc([0 1 2 3]);
MesaClearIRQ(1);

MesaWait(1);
        y(1)=MesaEnc([2], [pi*rd/2048]);            %Read Initial Values of Y
        y(2)=MesaEnc([0], [pi/2048])-pi;            %Need these values to go into the
        y(3)=MesaEnc([1], [-pi/2048])-pi;           %Correct Loop
MesaClearIRQ(1);
disp('Move Pendulum to Vert. Position');
pause(2);                                         %Pause so you can read the message       

%**Run Loop**
limit=0;
while(limit<intervalLim)

    while((abs(y(2))>1*pi/180) || (abs(y(3))>3*pi/180))
        MesaClearIRQ(1);
        MesaWait(1);                            %Only output message every three interrupts
        MesaClearIRQ(1);                        %To prevent Matlab from locking
        MesaWait(1);
        MesaClearIRQ(1);
        MesaWait(1);
        MesaClearIRQ(1);
        MesaWait(1);
        y(1)=MesaEnc([2], [pi*rd/2048]);
        y(2)=MesaEnc([0], [pi/2048])-pi;
        y(3)=MesaEnc([1], [-pi/2048])-pi;
        y  
        r
    end
    MesaClearIRQ(1);
    MesaClearEnc([2 3]);
    
    while(abs(y(1))<vLim && abs(y(2))<ang1Lim && abs(y(3))<ang2Lim && limit<intervalLim)
        MesaClearIRQ(1);
        MesaWait(1);                                %Wait on Sys
       % MesaClearIRQ(1);
       % MesaWait(1);
       
        y(1)=MesaEnc([2], [pi*rd/2048]);
        y(2)=MesaEnc([0], [pi/2048])-pi;
        y(3)=MesaEnc([1], [-pi/2048])-pi;
        %r = MesaEnc([3], [-.05/250])
        
        xbarb = w+Lpr*y;
        xa = y;
        u = -Ka*xa-Kb*xbarb+Nb*r;
        MesaPWM([2, u],24);                         %Output u 
        w = (coefw)*w+(coefu)*u+(coefy)*y;         
        
        %u = -K*xbar+Nb*r;
        %MesaPWM([2, u],24);                                   
        %xbar = (conmat)*xbar+gamma*u+Lp*y;
        %MesaClearIRQ(1); 
        limit = limit+1;
        %datay(:,limit)=y;
        %datau(:,limit)=u;
        %dataw(:,limit)=w;
    end
    MesaClearIRQ(1);  
    MesaPWM([2,0],24);                              %Clear Motor Voltage

    if(abs(y(1))>=vLim)
        disp('Motor Voltage Too High');
    end

    if(abs(y(2))>=ang1Lim)
        disp('Long Pend Out of Range');
    end
    
    if(abs(y(3))>=ang2Lim)
        disp('Short Pend Out of Range');
    end 
    
    if(limit>=intervalLim)
        disp('Time Limit Exceeded');
    end
    xbarb = [0 0 0]';
    u = 0;
    w = [0 0 0]';
    pause(2);                           %Pause so you can read error message
end

MesaClean(1);