close all
clear all
clear globals; 

global elast;
elast = 0;

p = PID_Shim; p.Configure;


dt = 0.005;
tf = 3;  %seconds

N = tf/dt;

t = NaN(N,1);
I = zeros(N,5);

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

i = 1;
t(1) = 0;
curr_time = 0;
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] = Plant2(i,t,I);          %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/.5;  %Convert control voltage to current
    
  end
end

disp('Done, plotting...');

figure('Position',[520 98 807 1000])
ah1 = subplot(2,1,1)
plot(t,I)
legend('Itarget','IConv','Irect','Ictrl','Idump');
xlabel('time (s)');
ylabel('Amps');

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

ah2 = subplot(2,1,2)
plot(t,err)
xlabel('time (s)');
ylabel('Error (Amps)');

linkaxes([ah1 ah2],'x');

