% 20-stage RC Filter
% Lecture #16
% Servo Compensators at DC

V = zeros(20,1);
dt = 0.01;
t = 0;
V0 = 0;

A = zeros(20,20);
for i=1:19
    A(i,i) = -50.1;
    A(i,i+1) = 25;
    A(i+1,i) = 25;
end
A(20,20) = -25;
B = zeros(20,1);
B(1) = 25;
C = zeros(1,20);
C(20) = 1;
D = 0;

% Servo Compensator
Z = 0;
Kx = [2 7 13 25];
Kz = 16;

Ref = 1;
n=0;
DATA = [];

while(t < 20)
   Ref = 1 + 0.25*sign(sin(0.314*t));
   V0 = -Kz*Z - Kx*V([5,10,15,20]);
 
   dV = A*V + B*V0;
   dZ = V(20) - Ref;
   
   V = V + dV * dt;
   Z = Z + dZ * dt;
   
   t = t + dt;

   n = mod(n+1, 5);
   if(n == 0)
      plot([0:20], [V0;V], 'b.-', t, 0, 'b+',20,Ref,'b+');
      ylim([0,2]);
      xlim([0,20]);
      pause(0.01);
      DATA = [DATA ; V(20), Ref];
   end
end

t = [1:length(DATA)]' * dt*5;
plot(t,DATA)