% satellite attitude control example
clear;
close all;

% We're not concerned with actual values: masses, frequency or time
% response specifics, thrust limits, etc.
%
% Model the plant simply. Here assume rotational moment of intetia is 1
J=1; 

% double integrator plant: From textbook example this is the 1/s^2 plant
nump=1;       % plant model numerator       
denp=[J 0 0]; % plant model denominator 

% lead compensator
% Our simple plant model has a "flat" phase at -180. An actual plant
% will have additional actuator poles that limit where we can place the
% lead zero, but not for our simple simulation model. Pick some convenient
% values for the lead zero and cancelling pole.

lz=1;   % lead zero
lp=10;  % lead-cancelling pole

num = [1 lz];               % lead-compensated plant numerator
den = conv([1 lp],[denp]);  % demonominator

% establish required gain for unity-gain crossover at max phase margin
[mag, ph,W] = bode(num,den);   
[maxph, idx] = max(ph);     % get the bode plot data phase peak
fcross      = W(idx)/(2*pi);
Kp = 1/mag(idx);            % gain required for unity gain crossover here.

% resultant compensated system model: apply the gain to the numerator.
num = Kp*num;

% Convert to state-space for the ordinary differential equation solver
% Matlab supplies (ode45 below)

[A,B,C,D] = tf2ss(num,den);

% now let's experiment with time-domain simulation techniques...

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% we'll capture the simulation in a video so initialize the file
vidfile = VideoWriter('satellite_attitude_sim.avi');
open(vidfile);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Initialize a capsule graphic...
% This is a bullet-shaped object that will animate the rotation
fig = figure('Name','Satellite attitude');
set(fig, 'Position', [10 10 1024 768])
subplot(2,3,1)
color = get(fig,'Color');
set(gca,'XColor',color,'YColor',color,'TickDir','out')

% verticies of the graphic
v = [1 -2; 1 2; 0 3.5; -1 2; -1 -2; 1 -2]

%initial position
startangle = 0; %-pi/2;
R = [cos(startangle) -sin(startangle); sin(startangle) cos(startangle)];

v = v*R;

% this is the thruster graphic
tlen=2
th = [0 0; tlen 0]
thr = th*R + [v(1,:); v(1,:)];
hPlot = plot(v(:,1),v(:,2), 'b-',0,0,'r-');
title('Capsule with Thrusters');
axis([-4 4 -4 4])
grid on
set(hPlot,{'LineWidth'},{2;10})
xlim([-4,4])
ylim([-4,4])
set(hPlot(1),'XData',[v(:,1)],'YData',[v(:,2)]);

MM = getframe(fig);
writeVideo(vidfile,MM);   

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% initialize the sim

ic = [0 0 0];    % initial conditions: zero rotational accel, vel, pos.
xout=[0 0 0];    % initialize the state output matrix
tend = 4;       % simulation end time
refin = pi;      % angle reference we're going to drive the capsule to

% tstep is the outer loop time-step below. This is our, "sampling rate".
% This is a continuous-time model to keep it simple, but you'll see below
% that the ODE solver is called in a loop with this as the time interval
% between iterations. If you make this time longer you will see the 
% deliterious effect of slow sampling rate on plant dynamics 
% because the solver is computing plant dynamics between iterations.
% but we're, "sampling" the angle and updating the output too slowly.
%
% This is, "aliasing" relative to the controller-plant bandwidth that
% is nominally the open-loop unity-gain crossover frequency we get from
% the Bode plot data above.
%
% Go ahead an play with this and you'll see dynamic performace get worse
% and eventually unstable.
%
% Here I pick a, "sampling rate" that is 50-times faster than the
% controller-plant model. This should nearly negate the effect
% of sampling rate on the plant, meaning the continuous-time model
% is valid: you shouldn't see the effect of sampling rate on the state
% dynamics plot. Slow this down (change 0.02 to 0.1, 0.5, etc.) and
% you will see the effect of sampling rate on continuous model dynamics.

tstep = .02/fcross;     


% These are some variables the loop will populate for plotting
tout=0;          % time output starting point
eout=0;          % error output init
refout=0;        
tm=0;  
yout = 0;  
yzohout = 0;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% initialize additional data plots

% Output Angle
subplot(2,3,2)
hPlotYout = plot(0,0,'r-',0,0,'b:',0,0,'k-');
legend('actual','desired','sampled');
title('Capsule Position');
ylabel('degrees');
xlabel('time(s)');
xlim([0,tend])

% command input
subplot(2,3,3)
hPlotErr = plot(0,0,'r-');
title('Sampled Position Angle Error');
ylabel('Degrees');
xlabel('time(s)');
xlim([0,tend])



% bode plot
subplot(2,3,5)
bode(num,den); 
grid on

% Root Locus for gain=1 up to our controller gain
subplot(2,3,6)
rlocus(num,den,[1:.001:Kp]);
grid on

% This just holds the capsule graphic still for a second on-screen
% before the controller is unleashed.
pause(1)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Control Loop Simulation

% loop until the specified simulation end time
% tm will increase by tstep before looping-back
while tm < tend,
   % each iteration will run the ODE solver for a timespan of tstep.
   % split this into ten increments for the solver. In this way the
   % solver is less discretized that our control loop, by a factor of 
   % 10 here. This can be thought of as simulating the, "real" plant
   % as a closer-to-continuous model as we sample the position and
   % adjust the thrusters at a slower rate.
   
   % the solver starts at zero-time each step. This doesn't matter so
   % long as the initial conditions are supplied correctly, which you
   % will see below.
   tspan = 0:tstep/10:tstep;

   err = (refin - yout);  % output error relative to reference input
   
   % pass this as command input to the solver. The cotroller gain is
   % applied within the solver
   u = err;               
   
   % invoke the ODE solver to compute state variables for each interval of
   % tspan. "rpcont" if where the state-space model lives so examine that.
   [t,x] = ode45(@(t,x) ssodefun(t,x,A,B,C,u), tspan, ic);
   
   % post-solver iteration data collection
   
   % the solver output time vector is zero-based. Offset by iteration
   % start time
   tt = tm+t;  
   
   tout = [tout; tt]; % append this iteration time vector to total sim time
   xout = [xout; x];  % append iteration states to sim state data
   
   sz = size(x);      % size of the state vector
   
   % Initial conditions for next iteration are the states of the last
   % output of the solver for this iteration
   ic = x(sz(1),:);   
   
   % error for entire iteration interval is constant because we only
   % "sample" it at the iteration loop rate
   et(1:sz(1))   = err;
   eout = [eout et];  % append the error vector for this iteration.
   
   % Same for the reference input
   rt(1:sz(1)) = refin;
   refout = [refout rt];
   
   % Apply the state-space 'C' matrix to the state matrix to obtain
   % angular output for this iteration
   yo = C*x';
   
   % the "sampled" output for the next iteration is that last
   % position solved in this iteration.
   yout = yo(sz(1));
   
   % the "sampled" position is the last value for this iteration
   yzoh(1:sz(1)) = yo(sz(1));  
   yzohout = [yzohout yzoh];
    
   R = [cos(yout) -sin(yout); sin(yout) cos(yout)];
   vo = v*R;     

   % scale the thruster with the error
   thruster_scale = 4;
   th = [0 0; err*thruster_scale 0]
   
   % paint the thruster out the proper side of the capsule...
   % rotate the trust vector and offset it to the proper corner of the
   % capsule.
   if err>0,
    thr = th*R + [vo(1,:); vo(1,:)];
   else
    thr = th*R + [vo(5,:); vo(5,:)];    
   end
   
   % set the capsule graphic plot data for this iteration
   set(hPlot(1),'XData',vo(:,1),'YData',vo(:,2));
   set(hPlot(2),'XData',thr(:,1),'YData',thr(:,2));
   
   % paint the capsule position and reference plot
   yy = C*xout'
   set(hPlotYout(1),'XData',tout,'YData',yy*180/pi);
   set(hPlotYout(2),'XData',tout,'YData',refout*180/pi);
   set(hPlotYout(3),'XData',tout,'YData',yzohout*180/pi);
   
   % paint the current error plot
   set(hPlotErr,'XData',tout,'YData',eout*180/pi);
     
   % set the start time tm for the next iteraton.
   tm = tm + tstep;

   % write the figure frame to the video file
   MM = getframe(fig);
   writeVideo(vidfile,MM);
   
  % loop back for the next iteration
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% simulation complete

close(vidfile);

