close all
clear all
clear globals; 

Kp = .1;
Kd = .001;

N = 3;

global elast;
elast = 0;

p = PID_Shim; p.Configure;

Id = 1;


tf = 1;
It = 1;  %Amps

elast = 0;
t = 0;
tic
while(1)
  [err Ic] = p.GetADC(1);
  [err Ir] = p.GetADC(0);

  Ic = (Ic-.505)/.18;
  Ir = (Ir-.332)/.12;

  t0 = t;
  t = toc;
  It = 1+sin(2*pi*t/10);
  
  dt = t-t0;


  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;
  %SId = .5*Id/(2*N/3)+.25;
  if(SId > .76) SId = .75;  end
  if(SId < .24) SId = .24;  end
  p.Set_PWM_DutyCycle(SId);

  disp(['Ir = ' num2str(Ir)  '  Ic = ' num2str(Ic)  '     dt = ' num2str(dt)  '  ctrl = ' num2str(ctrl)]);

end