function [] = pid(InputVal, SampleTime, k_p, k_d, k_i)
% runs the satellite, plots some graphs, and saves some data
% usage: pid(InputVal, SampleTime)
%
% Engineering 58 Lab 5
% David Luong, Mark Piper, Adem Kader, Aron Dobos
%

% Set up sampling parameters
SampleFreq=40;         %Sampling Frequency

if SampleTime*SampleFreq >= 2000 
    return
end

NumPts=SampleFreq*SampleTime;             %Number of points in run (<=2000).
PreSample=0;           %Number of points before control
                        %  algorithm runs (this generates
                        %  baseline data);
InChan=0;               %Input channel to use on DAQ board
OutChan=0;              %Output channel to use on DAQ board
DT=1/SampleFreq;        %Sampling Interval

%Setup the input channel 
%You shouldn't need to change this.
Ain = analoginput('nidaq');
Ain.SampleRate=SampleFreq;
Ain.SamplesPerTrigger=NumPts;
Ain.InputType='SingleEnded';
Ain.BufferingConfig=[1  2000];
Ain.TransferMode='Interrupts';
addchannel(Ain,InChan); 

%Setup the output channel
%You shouldn't need to change this.
Aout = analogoutput('nidaq');
addchannel(Aout,OutChan);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%% Predefine any arrays to make loop quicker %%%%%%%%
%%%%%%%%%%%%%%%%%%%%% Change as needed %%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
SolarVoltage=zeros(NumPts,1);
output=zeros(NumPts,1);
errsig=zeros(NumPts, 1);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%Define Time Vector (time=0 after PreSample is done)
t=((1:NumPts)-PreSample)*DT;

LastIndex=0;
start(Ain);

% turn off the motor initially - the controller will turn it on as
% necessary
putsample(Aout,0);  
% apply a step input to the motor.  
% to the motor leads.


% PID controller constants
%k_p = 1;
%k_d = 1;
%k_i = 1;

integrator = 0;

while (Ain.SamplesAcquired<NumPts),
    j=Ain.SamplesAcquired;
    
    while(LastIndex==j),            %Wait for next datum.
        j=Ain.SamplesAcquired;
    end
    if ((j-LastIndex)~=1),          %Make sure no data was missed.
        disp(sprintf('Warning, datum skipped: %d, %d',LastIndex,j));
    end
    InData=peekdata(Ain,j);         %Retrieve most recent datum.
    
    if (t(j)<0),        %If during PreSample, output 0.
        SolarVoltage(j)=0; 
    else                %Else, do control algorithm
        SolarVoltage(j) = InData(j);        
    end
    
    % calculate the error signal
    errsig(j) = InputVal - SolarVoltage(j);
    
    % run the integrator
    
    % calculate each of the controller output values
    proportional = k_p*errsig(j);
    if (j>1)
        derivative = k_d*(errsig(j)-errsig(j-1))/DT;
        integrator = integrator + (errsig(j)+errsig(j-1))*DT/2;
    else
        integrator = integrator + errsig(j)*DT/2;
        derivative = k_d*errsig(j);
    end
    
    integral = k_i*integrator;
    
    % apply the output of the three controllers
    output(j) = -1*(proportional+derivative+integral);
    putsample(Aout, output(j));
    
    LastIndex=j;
end

% turn off the motor
putsample(Aout,0);  

%Free up any memory that we used.
delete(Ain)
clear Ain
delete(Aout)
clear Aout

plot(t, SolarVoltage, t, errsig, t, output)
legend('Cell Voltage', 'ErrSig', 'D/A Output');
xlabel('Time');
ylabel('Voltage');

title(sprintf('PID InputVal=%.2f, k_p=%d k_d=%d k_i=%d', InputVal, k_p, k_d, k_i));
filename = sprintf('pid_%.2f_%d_%d_%d.bmp', InputVal, k_p, k_d, k_i);
print('-dbitmap', filename);
filename = sprintf('pid_%.2f_%d_%d_%d.mat', InputVal, k_p, k_d, k_i);
save(filename,'t','SolarVoltage', 'InputVal', 'SampleTime', 'errsig', 'output', 'DT', 'k_p', 'k_i', 'k_d');

beep;