close all
clear all
clear globals; 

global elast;
elast = 0;

%p = PID_Shim; p.Configure;


dt = 0.005;
tf = 2;  %seconds

N = tf/dt;

t = NaN(N,1);
I = NaN(N,4);

I(1,3) = 0;
I(1,4) = 0;

t(1) = 0;
curr_time = 0;

for i = 1:N
   
  t(i) = i*dt;

  if(i<N/2)
    ctrl = 1;
  else
    ctrl = 0;
  end
  I(i,2) = 1;
  I(i,1) = 1;
  I(i,4) = ctrl;
  
  [I(i,5) I(i,3)] = Plant(I(i,2), I(i,1), ctrl, curr_time);  

    
end



tic;
while(curr_time < tf)    
curr_time = toc;
  if(curr_time-t(i) >= dt)    
    i = i+1;
    t(i) = curr_time;

    I(i,1) = It(curr_time);   %Get prescribed values for It
    I(i,2) = Ic(curr_time);   %Get prescribed values for Ic

    [I(i,5) I(i,3)] = Plant(I(i,2), I(i,1), I(i-1,4), curr_time);         %Estimate Ir from plant model and the most recent control value.
    
                              %Update control value
    [inv ctrl clipped] = PID(I(i,3),I(i,2), I(i,1));            %(Compute in Matlab)
    %[inv ctrl clipped] = p.Test(I(i,3),I(i,2), I(i,1))         %(or let the hardware do it)
                             
    I(i,4) = ctrl;
    
  end
end


figure
plot(t,I)
legend('Itarget','IConv','Irect','Ictrl','Idump');
xlabel('time (s)');
ylabel('Amps');


err = I(:,1)-I(:,3);

figure
plot(t,err)
xlabel('time (s)');
ylabel('Amps');