close all
clear all
clear globals; 

p = PID_Shim; p.Configure;



thres1 = -.25;
thres2 = -.5;


It = .5;  %Amps

SIt = .5*It/15+.25
if(SIt > .75) SIt = .75;  end
if(SIt < .25) SIt = .25;  end
p.Set_PWM_DutyCycle(SIt);
  
tic;
while(1)
 t = toc;
 
  %It = .25+.5*sin(2*pi*t/10);
  %SIt = .5*It/15+.5;
  %if(SIt > .75) SIt = .75;  end
  %if(SIt < .25) SIt = .25;  end
  %p.Set_PWM_DutyCycle(SIt);
  
  
  
  
 [err Ir] = p.GetADC(0);
 [err Ic] = p.GetADC(1);

 [err Ctrl_clipped] = p.GetADC(2);
 [err Ctrl] = p.GetADC(3);

 
 Ir = (Ir-.332)/.12;   %scale all measured quantities to Amps
 Ic = (Ic-.500)/.185;   
 Ctrl_clipped = Ctrl_clipped*2;
 Ctrl = Ctrl*2;
 
 if(Ctrl < thres2)  %Disable Both
     DisableConverters(p,0);
     DisableConverters(p,1);
 end
 if((Ctrl < thres1)  && (Ctrl> thres2))  %Disable just one
      EnableConverters(p,0);
      DisableConverters(p,1);  
 end
 
 if(Ctrl > thres1)  %Enable both
     EnableConverters(p,0);
     EnableConverters(p,1);
 end
   
 
 disp(['It = ' num2str(It,'%f8') '   Ic = ' num2str(Ic,'%f8')   '   Ir = ' num2str(Ir,'%f8')  '   Ctrl = ' num2str(Ctrl,'%f8')   '   Ctrl_c = ' num2str(Ctrl_clipped,'%f8')]);
 
end