close all
clear all
clear globals; 

Kp = 0;
Kd = 0;

Nmods = 3;

global elast;
elast = 0;

p = PID_Shim; p.Configure;


tf = 1;
It = 1;  %Amps

elast = 0;
t = 0;
tic
while(t<tf)
  Ic = GetADC(channel);
  Ir = GetADC(channel);

  Ic = (Ic-.522)/.18;
  Ir = (Ir-.337)/.12;

  t0 = t;
  t = toc;



  FF = It-Ic;
  e = It-Ir;
  edot = (e-elast)/(t-t0);
  elast = e;
  ctrl = FF+Kp*e-Kd*edot;

  %SId = .5*ctrl/(2*N/3)+.25;
  %if(SId > .76) SId = .75;  end
  %if(SId < .24) SId = .24;  end
  %Set_PWM_DutyCycle(SId);

  disp(['Ir = ' num2str(Ir)

end