% Script file for running autopilot given gains for PID controller.
%
% Aircraft is simulated as two-pole system.
%
% User must enter PID coefficients as in following example:
% pid = [3, 5,2];
%
% Then user types the word autopilot
%
% A plot is made of a desired altitude path and actual path.
%
% 

% Specify time at 0 to 100 seconds.
t = 1 : 100;

% Specify plane as digital filter.
% y(n) = b0 * x(n) - a1 * y(n-1) - a2 * y(n-2)
%  where x = input, y = actual altitude
a1 = -0.95;  % Stable if |a1| < 1 + a2
a2 = 0.01;  % Stable if |a2| < 1
b0 = 1;

% Specify desired trajectory as sigmoid curve starting at zero and
%  leveling off at 10,000.
desired = 5000*(1 - cos(pi*t/100));

% Plot desired trajectory
plot(t,desired,'k-')

% Plot opening plane must pass thru
hold on
plot([100,100],[0,9000],'r-')
plot([100,100],[11000,20000],'r-')

% Initialize variables.
sum_over_time = 0.0;
prev_errsig = 0.0;
actual = [];

% Use loop to step thru time
for t_index = t
  % Calculate altitude error.
  if t_index > 1
    errsig = desired(t_index - 1) - actual(t_index - 1);
  else
    errsig = 0.0;
  end

  % Calculate system input as PID of errsig.
  % P part is just errsig multiplied by P gain.
  psig = errsig * pid(1);
  
  % I part is just sum of errsig over time multiplied by I gain.
  sum_over_time = sum_over_time + errsig;
  isig = sum_over_time * pid(2);
  
  % D part is just the change in errsig multiplied by D gain.
  err_change = errsig - prev_errsig;
  dsig = err_change * pid(3);
  
  % Add the feedback signals together to get input signal.
  input(t_index) = psig + isig + dsig;
  
  % Run digital filter representing plane.
  if t_index > 2
    actual(t_index) = b0 * input(t_index) ...
                      - a1 * actual(t_index - 1) ...
                      - a2 * actual(t_index - 2);
  else
    if t_index > 1
      actual(t_index) = b0 * input(t_index) ...
                        - a1 * actual(t_index - 1);
    else
      actual(t_index) = b0 * input(t_index);
    end
  end
end

% Plot actual position of plane
plot(t,actual,'b-')

hold off

% Check whether plane went through opening.
if abs(actual(100) - 10000) < 1000
  display('Success')
else
  display('Crash')
end