% MatLab template file for MAE171A/MAE175A to help you
% (1) simulate open loop (uncontrolled) behavior of model
% (2) design controller (internal PID or via rltool) and simulate 
% closed-loop behavior of model and controller
% Notes:
% - Edit model parameters in the (default) parameter file PARAMETERS.M
% - When using rltool for control design, export the controller to
%   workspace using the variable name C

% Written by R.A. de Callafon, Dept. of MAE, UCSD (2004)

clc
drawnow;
format compact

% EDIT THE PARAMETERS OF YOUR MODEL IN THE DEFAULT FILE PARAMETERS.M
% Ask for the filename with the parameters:
disp('- Welcome to MAELAB')
cantfindit=1;
while cantfindit,
    cantfindit=0;
    typo=1;
    while typo==1,
        typo=0;
        if exist('parfile')==1,
            parfilep=input(['Name of file with model parameters [''' parfile ''']: ']);
            if ~isempty(parfilep),
                parfile=parfilep;
            end
        else
            parfile=input('Name of file with model parameters [''parameters'']: ');
            if isempty(parfile),
                parfile='parameters';
            end       
        end
        if abs(parfile(1))<65,
            typo=1;
            clear parfilep parfile
            disp(char(7))
            disp(['==> Sorry, incorrect filename, choosing default name ''parameters''']);            
        end
    end
    if exist('parfile')==1,
        lfname=length(parfile);
        if parfile(lfname-1)~='.', parfile=[parfile '.m']; end
        eval(['testje=exist(''' parfile ''');']);
        if testje~=2,
            disp(['==> Sorry, cannot find ' parfile ]);
            cantfindit=1;
        end
        lfname=length(parfile);
        parfile=parfile(1:lfname-2);
    end
end


% Evoke parameter file
disp(['Evoking ' parfile '.m to update your model parameters']);
eval(parfile);
        

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% The parameters and the transfer function given below is based on the following model:
% We assume a 2DOF system of two masses/inertias m1 and m2. The two masses/inertia
% are connected via a spring k2 while m1 is connected via a spring k1 to the ground.
% Each of the masses/inertia are connected to the ground via a damping of respectively
% d1 and d2. A force/torque is applied to mass m1 only.
% The position/rotation of the mass/inertia m1 is denoted by x1
% The position/rotation of the mass/inertia m2 is denoted by x2
% The voltage applied to the electromotor is denoted by u and is proportional 
% to the generated force/torque.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% The model below is a transfer function from control effort u (measured in voltage)  %
% to the position/rotation x1 or x2 of mass/inertia m1 or m2 (measured in counts).                %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


typo=1;
while typo==1,
    if exist('encoderout')==1,
        encoderoutep=input(['Specify encoder output (1/2) [' num2str(encoderout) ']: ']);
        if ~isempty(encoderoutep),
            encoderout=encoderoutep;
        end
    else
        encoderout=input(['Specify encoder output (1/2): ']);
        while isempty(encoderout),
            clear encoderout
            encoderout=input(['Specify encoder output (1/2): ']);
        end
    end
    if ~((encoderout==1)|(encoderout==2)),
        clear encoderout
    else
        typo=0;
    end
end    
    
    


% model is given by a numerator `numg' and denominator `deng'
% and are specified here:
numg1=[m2 d2 k2]; % for x1 as output
numg2=k2;         % for x2 as output
deng=[m1*m2 (m1*d2+m2*d1) (k2*m1+(k1+k2)*m2+d1*d2) ((k1+k2)*d2+k2*d1) k1*k2];
if encoderout==1,
    numg=numg1;       % we're using x1 as output
else
    numg=numg2;       % we're using x2 as output
end
clear numg1 numg2

% to include dynamics of the DC-motor (actuator), the total denominator becomes
deng0=deng;
deng=conv(deng,[1/209 1]);

% to include the hardware gain, the total numerator becomes
numg0=numg;
Khw=9.7656e-3; % hardware gain, only plays a role in controller implementation
numg=Khw*numg;


% For Matlab 5.3 and higher in case students like to use root locus method
% for control design:
G=tf(numg,deng); % includes dynamics of mechanics + motor + hardware gain


% In order to simulate Bode plots,
% a frequency vector is specified here:
f=logspace(-2,2,500);      % in Herz, logarithmically spaced



% creating figures

for figs=1:6,
   figure(figs);
   set(figs,'Visible','off');
end

%
% Now we enter the menu.
% Please search throughout the template file for the different
% options to see what is going on
%


   disp(' ');
   disp('- Choose an option:');
   disp(' ');
   disp('(1) Bode plot of G(s) ');   
   disp('(2) Simulate/compare open loop step response ');
   disp('(3) Simulate/compare open loop impulse response');
   disp('(4) Design/evaluate feedback controller');
   disp('(5) Simulate closed loop response');
   disp('(6) Exit this menu');


option=0;
while option==0,
   option=input('Enter your choice: ');
   if isempty(option), option=0; end
   option=round(max(option));
   if option>6, option=0; end
   if option<1, option=0; end
end

if option==1,

   disp('- Mechanical resonance modes of the model G(s):');
   damp(deng0);
   disp('Note: Model G(s) also contains motor dynamics and is available')
   disp('      in the Matlab workspace via the variable G')
    
   %
   % Here we compute frequency response
   %
   [mag,pha]=bode(numg,deng,2*pi*f);
   
   %
   % Here we plot the Bode response response
   %
   figure(1)
   plttitle='Bode plot of G(s)';
   set(1,'Userdata',plttitle');
   set(1,'Visible','on');
   clf % use to be clg
   subplot(2,1,1);
   loglog(f,mag);
   title(plttitle)
   xlabel('frequency [Hz]');
   ylabel('magnitude')
   grid
   subplot(2,1,2);
   semilogx(f,pha);
   xlabel('frequency [Hz]');
   ylabel('phase [deg]');
   grid
   disp(['- Figure 1: ' plttitle])   
   
end


if option==2,

   if exist('stepsize')==1,
      stepsizep=input(['Enter (open loop) step size [' num2str(stepsize) ' Volts]: ']);
      if ~isempty(stepsizep),
          stepsize=stepsizep;
      end
   else
      stepsize=input('Enter (open loop) step size [Volts]: ');
      while isempty(stepsize),
          clear stepsize
          stepsize=input('Enter (open loop) step size [Volts]: ');
      end
   end
   if exist('dwelltime')==1,
      dwelltimep=input(['Enter dwell time [' num2str(dwelltime) ' msec]: ']);
      if ~isempty(dwelltimep),
          dwelltime=dwelltimep;
      end
   else
      dwelltime=input('Enter dwell time [msec]: ');
      while isempty(dwelltime),
          clear dwelltime
          dwelltime=input('Enter dwell time [msec]: ');
      end
   end
   stepsize=abs(stepsize(1));
   dwelltime=abs(dwelltime(1));

   typo=1;
   while typo==1,
      typo=0;
      if exist('sfilename')~=1,
        sfilename=input('File that contains step experiment [ENTER for none]: ');
      else
        osfilename=sfilename; 
        sfilename=input(['File that contains step experiment [ENTER for ''' sfilename ''' or 0 for none]: ']);
        if length(sfilename)==0,
          sfilename=osfilename;
        end
      end  
      if length(sfilename)~=0,
          if abs(sfilename(1))==0,
              clear sfilename osfilename              
          elseif abs(sfilename(1))<65
              typo=1;
              if exist('osfilename')==1,
                  sfilename=osfilename;
              else
                  clear sfilename
              end
          end
      else
          clear sfilename
      end
      if exist('sfilename')==1,
        lfname=length(sfilename);
        if sfilename(lfname-1)~='.', sfilename=[sfilename '.m']; end
        eval(['testje=exist(''' sfilename ''');']);
        if testje~=2,
            disp(['==> Sorry, cannot find ' sfilename ]);
            typo=1;
            clear sfilename osfilename              
        else
            lfname=length(sfilename);
            sfilename=sfilename(1:lfname-2);
        end
      end            
   end        

   if exist('sfilename')==1,
       
      lfname=length(sfilename);
      if sfilename(lfname-1)~='.', sfilename=[sfilename '.m']; end
       
      disp(['- Evoking ' sfilename ' to define t and y from step experiment']);
      lfname=length(sfilename);
      sfilename=sfilename(1:lfname-2);
      clear t y
      eval(sfilename);
      if (exist('y')~=1)|(exist('t')~=1),
          disp('==> Sorry, incorrect file format: no output y or time vector t defined');
          disp('    Skipping comparison!')
          t=[];y=[];
      end      
   
   else
       
       t=[];y=[];
   
   end
   
   disp('- Computing step response of G(s)');
   %
   % Here we compute step response
   %
   tstep=linspace(0,2*1e-3*dwelltime,900)';
   ustep=[stepsize*ones(450,1);0*ones(450,1)];   
   % the step response without the hardware gain Khw that only plays a role in
   % feedback controller implementation
   ystep=lsim(numg0,deng,ustep,tstep);

   %
   % Here we plot the time response
   %
   figure(2)
   if length(y)>0,
       plttitle='Simulated (solid) and measured (dashed) open loop step response';
   else
       plttitle='Simulated open loop step response';
   end
   set(2,'Userdata',plttitle');
   set(2,'Visible','on');
   clf % use to be clg

   plot(tstep,ystep,t,y,':');
   title(plttitle);
   xlabel('time [sec]');
   ylabel('output signals (solid, dashed)')
   disp(['- Figure 2: ' plttitle]);

end


if option==3,
    
   if exist('imptime')==1,
      imptimep=input(['Enter length of impulse response [' num2str(imptime) ' msec]: ']);
      if ~isempty(imptimep),
          imptime=imptimep;
      end
   else
      imptime=input('Enter length of impulse response [msec]: ');
      while isempty(imptime),
          clear imptime
          imptime=input('Enter length of impulse response [msec]: ');
      end
   end
   if exist('deltime')==1,
      deltimep=input(['Enter delay of impulse excitation [' num2str(deltime) ' msec]: ']);
      if ~isempty(deltimep),
          deltime=deltimep;
      end
   else
      deltime=input('Enter delay of impulse excitation [msec]: ');
      while isempty(deltime),
          clear deltime
          deltime=input('Enter delay of impulse excitation [msec]: ');
      end
   end    
   if exist('impscale')==1,
      impscalep=input(['Enter scaling of impulse response [' num2str(impscale) ']: ']);
      if ~isempty(impscalep),
          impscale=impscalep;
      end
   else
      impscale=input('Enter scaling of impulse response [1]: ');
      if isempty(impscale),
          impscale=1;
      end
   end

   
   
   imptime=abs(imptime(1));
   deltime=abs(deltime(1));
   if deltime>imptime,
       deltime=imptime;
   end
   
   typo=1;
   while typo==1,
      typo=0;
      if exist('ifilename')~=1,
        ifilename=input('File that contains impulse experiment [ENTER for none]: ');
      else
        oifilename=ifilename; 
        ifilename=input(['File that contains impulse experiment [ENTER for ''' ifilename ''' or 0 for none]: ']);
        if length(ifilename)==0,
          ifilename=oifilename;
        end
      end  
      if length(ifilename)~=0,
          if abs(ifilename(1))==0,
              clear ifilename oifilename              
          elseif abs(ifilename(1))<65
              typo=1;
              if exist('oifilename')==1,
                  ifilename=oifilename;
              else
                  clear ifilename
              end
          end
      else
          clear ifilename
      end
      if exist('ifilename')==1,
        lfname=length(ifilename);
        if ifilename(lfname-1)~='.', ifilename=[ifilename '.m']; end
        eval(['testje=exist(''' ifilename ''');']);
        if testje~=2,
            disp(['==> Sorry, cannot find ' ifilename ]);
            typo=1;
            clear ifilename oifilename              
        else
            lfname=length(ifilename);
            ifilename=ifilename(1:lfname-2);
        end
      end            
   end        

   if exist('ifilename')==1,
       
      lfname=length(ifilename);
      if ifilename(lfname-1)~='.', ifilename=[ifilename '.m']; end
       
      disp(['- Evoking ' ifilename ' to define t and y from impulse experiment']);
      lfname=length(ifilename);
      ifilename=ifilename(1:lfname-2);
      clear t y
      eval(ifilename);
      if (exist('y')~=1)|(exist('t')~=1),
          disp('==> Sorry, incorrect file format: no output y or time vector t defined');
          disp('    Skipping comparison!')
          t=[];y=[];
      end      
   
   else
       
       t=[];y=[];
   
   end
   
   disp('- Computing (scaled) impulse response of G(s)');
   %
   % Here we compute impulse response
   %
   timp=linspace(0,1e-3*imptime,900)';  
   % the impulse response without the hardware gain Khw that only plays a role in
   % feedback controller implementation
   yimp=impulse(numg0,deng,timp)*impscale;
   index=find(timp<=deltime*1e-3);
   yimp=[zeros(length(index),1);yimp(1:length(yimp)-length(index))];
   %
   % Here we plot the time response
   %
   figure(3)
   if length(y)>0,
       plttitle='Simulated (solid) and measured (dashed) impulse response';
   else
       plttitle='Simulated impulse response';
   end
   set(3,'Userdata',plttitle');
   set(3,'Visible','on');
   clf % use to be clg

   plot(timp,yimp,t,y,':');
   title(plttitle);
   xlabel('time [sec]');
   ylabel('output signals (solid, dashed)')
   disp(['- Figure 3: ' plttitle]);

end




if option==4,
    
    usethiscontrol=0;
    if exist('C')==1,
        usethiscontrol=2;
        while ((usethiscontrol~=0)&(usethiscontrol~=1)),
            usethiscontrol=input('Controller C found in memory: use this controller? [1/0]: ');
            if isempty(usethiscontrol), 
                usethiscontrol=2; 
            end
        end
    end
    
    % Note: tau is needed and introduced to make controller realizable
    %       for the differentiating part

   tau=2*pi*0.0001;
    
    
if usethiscontrol==0,        

   disp('- Enter the control design variables');
   if exist('kp')==1,
      kpp=input(['Give proportional gain kp [' num2str(kp) ']: ']);
      if ~isempty(kpp),
          kp=kpp;
      end
   else
      kp=input('Give proportional gain kp: ');
      while isempty(kp),
          clear kp
          kp=input('Give proportional gain kp: ');
      end
   end
   if exist('kd')==1,
      kdp=input(['Give proportional gain kd [' num2str(kd) ']: ']);
      if ~isempty(kdp),
          kd=kdp;
      end
   else
      kd=input('Give proportional gain kd: ');
      while isempty(kd),
          clear kd
          kd=input('Give proportional gain kd: ');
      end
   end
   if exist('ki')==1,
      kip=input(['Give proportional gain ki [' num2str(ki) ']: ']);
      if ~isempty(kip),
          ki=kip;
      end
   else
      ki=input('Give proportional gain ki: ');
      while isempty(ki),
          clear ki
          ki=input('Give proportional gain ki: ');
      end
   end
   kp=abs(kp(1));
   kd=abs(kd(1));
   ki=abs(ki(1));
   

   % General form of controller.
   numc=[kd kp ki];
   denc=[tau 1 0];

   % Clearly, if ki = 0, we can remove the pole and zero at 0 from the
   % the general form of the controller, since we have a PD controller

   if ki==0,
      numc=[kd kp];
      denc=[tau 1];
   end
   
   % For matlab 5.3 and higher
   Cpid=tf(numc,denc);
   
else
    
    % get the information from the controller in workspace
    [numc,denc]=tfdata(C);
    numc=numc{1};
    denc=denc{1};
    
    % add the extra time constant to the controller
    denc=conv(denc,[tau 1]);
    
    % For matlab 5.3 and higher (update the controller)
    Cpid=tf(numc,denc);    
   
end   
   
   disp('- Controller transfer function (including motor dynamics):');
    Cpid
   disp('- Computing controller and closed loop Bode plots');   

   % computing Bode plot of controller
   [magc,phac]=bode(numc,denc,2*pi*f);

   % computing loop gain
   [numl,denl]=series(numg,deng,numc,denc);
   % computing Bode plot of loop gain
   [magl,phal]=bode(numl,denl,2*pi*f);
   [rel,iml]=nyquist(numl,denl,2*pi*f);

   % computing closed loop output connection (using negative feedback connection)
   [numcl,dencl]=cloop(numl,denl,-1);
   % computing closed loop input connection (using negative feedback connection)
   dens=dencl;
   nums=conv(numc,deng);
   % computing Bode plot of closed loop transfer function
   [magcl,phacl]=bode(numcl,dencl,2*pi*f);

   % check stability
   stability=1;
   clpoles=roots(dencl);
   if max(real(clpoles))>0,
       stability=0;
       disp('==> CLOSED LOOP UNSTABLE');
   end


   
   %
   % Here we plot the Bode plot of the controller
   %
   figure(4)
   plttitle='Bode plot of controller C(s)';
   set(4,'Userdata',plttitle');
   set(4,'Visible','on');
   clf % use to be clg

   subplot(2,1,1);
   loglog(f,magc);
   title(plttitle)
   xlabel('frequency [Hz]');
   ylabel('magnitude')

   subplot(2,1,2);
   semilogx(f,phac);
   xlabel('frequency [Hz]');
   ylabel('phase [deg]');
   disp(['- Figure 4: ' plttitle]) 

   %
   % Here we plot the Nyquist plot and closed-loop transfer function
   %
   figure(5)
   plttitle='Frequency plots of loopgain L(s)';
   set(5,'Userdata',plttitle');
   set(5,'Visible','on');
   clf % use to be clg

   subplot(2,2,1);
   loglog(f,magl);
   hold on
   loglog([f(1) f(length(f))],[1 1],':')
   hold off
   title('magnitude L(s)')
   xlabel('frequency [Hz]');
   ylabel('magnitude')
   
   axiss=axis;
   axis([f(1) f(length(f)) axiss(3) axiss(4)]);

   subplot(2,2,2);
   plot(rel,iml);
   axis([-4 4 -4 4]);
   hold on
   plot(-1,0,'*')
   hold off
   title('Nyquist L(s)')
   xlabel('real')
   ylabel('imag')

   subplot(2,2,3);
   semilogx(f,phal);
   hold on
   semilogx([f(1) f(length(f))],[-180 -180],':');
   hold off
   xlabel('frequency [Hz]');
   ylabel('phase [deg]');
   title('Phase L(s)')
   axiss=axis;
   axis([f(1) f(length(f)) axiss(3) axiss(4)]);

   subplot(2,2,4);
   loglog(f,magcl);
   hold on
   loglog([f(1) f(length(f))],[1 1],':')
   hold off
   title('magnitude L(s)/(1+L(s))')
   xlabel('frequency [Hz]');
   ylabel('magnitude')
   axiss=axis;
   axis([f(1) f(length(f)) axiss(3) axiss(4)]);

   if stability==1,
      text(0.1,0.1,'closed loop stable','color','green','Units','normalized');
   else
      text(0.1,0.1,'closed loop unstable!','color','red','Units','normalized');
   end

   disp(['- Figure 5: ' plttitle])

end

if option==5,    

   if exist('stability')==0,
      disp('==> First run option 4 to evaluate the controller ');
   else
       
        disp('- Checking closed-loop stability fo previously evaluated controller');   
              
        % recheck stability in case you changed the encoder output...       
        % computing loop gain
        [numc,denc]=tfdata(Cpid);
        numc=numc{1};denc=denc{1};
        [numl,denl]=series(numg,deng,numc,denc);

        % computing closed loop output connection (using negative feedback connection)
        [numcl,dencl]=cloop(numl,denl,-1);

        % check stability
        stability=1;
        clpoles=roots(dencl);
        if max(real(clpoles))>0,
            stability=0;
            
        end
       
       
      if stability==1,
          if exist('clstepsize')==1,
              clstepsizep=input(['Enter (closed loop) step size [' num2str(clstepsize) ' counts]: ']);
              if ~isempty(clstepsizep),
                  clstepsize=clstepsizep;
              end
          else
              clstepsize=input('Enter (closed loop) step size [counts]: ');
              while isempty(clstepsize),
                  clear clstepsize
                  clstepsize=input('Enter (closed loop) step size [counts]: ');
              end
          end
          if exist('cldwelltime')==1,
              cldwelltimep=input(['Enter dwell time [' num2str(cldwelltime) ' msec]: ']);
              if ~isempty(cldwelltimep),
                  cldwelltime=cldwelltimep;
              end
          else
              cldwelltime=input('Enter dwell time [msec]: ');
              while isempty(cldwelltime),
                  clear cldwelltime
                  cldwelltime=input('Enter dwell time [msec]: ');
              end
          end
          clstepsize=abs(clstepsize(1));
          cldwelltime=abs(cldwelltime(1));
   
          cltstep=linspace(0,2*1e-3*cldwelltime,900)';
          clustep=[clstepsize*ones(450,1);0*ones(450,1)]; 

          % closed loop step response of output
          ystepcl=lsim(numcl,dencl,clustep,cltstep);
          
         % closed loop step response of input
          ustepcl=Khw*lsim(nums,dens,clustep,cltstep);

          figure(6)

          plttitle='Closed loop simulated step response';
          set(6,'Userdata',plttitle');
          set(6,'Visible','on');
          clf % use to be clg
          subplot(2,1,1);
          plot(cltstep,clustep,'--',cltstep,ystepcl,'-');
          title(plttitle);
          xlabel('time [sec]');
          ylabel('reference (dashed), output (solid)')
          subplot(2,1,2);
          plot(cltstep,ustepcl);
          xlabel('time [sec]');
          ylabel('control signal [V]')
          %axis([cltstep(1) cltstep(length(cltstep)) -5 5]);

          disp(['- Figure 6: ' plttitle])          
      else
          disp('==> CLOSED LOOP SYSTEM UNSTABLE');
          disp(['    Redesign controller around encoder ' num2str(encoderout) ' with option 4']);
          set(6,'Visible','off')
      end
   end
end

if option==6,

   disp('- Exciting MAELAB...');

end

clear typo cantfindit deltatimep deltimep imptimep index lfname figs numg deng f
clear option parfilep testje mag pha plttitle dwelltimep stepsizep
clear axiss numc denc numl denl
clear rel iml magc magcl magl phac phacl phal tau
clear kpp kdp kip