% readplot_exp9.m: Reads the data generated by
% main-prog-exp9.c and outputs a polynomial that is used as
% the compensator in main-prog.c. 
%
% PRECONDITIONS: The number of steps in readplot_exp9.m should
% equal the number of steps in main-prog-exp9.c.
%
% User needs to ensure that main-prog-exp9.c has worked the uC
% for at least 32 seconds. Only then will the user find 16 
% steps in the plots of ud and u. Then, ud is stepped through 
% 16 steps from 0.5V to 8V at an interval of 0.5V.
%  
% If the user wishes to step through N steps, then the user 
% should ensure that main-prog-exp9.c works the uC for N*2 
% seconds. 
%
% Apart from the number of steps, the time duration of steps 
% also matters. main-prog-exp9.c holds ud at each step for 2
% seconds i.e. 1000 data points. Therefore, readplot_exp9.m is
% written according to 1000 data points per step.  
% 
% POSTCONDITIONS: 3 figures are generated: Raw data of ud and u 
% vs. t, uavg and ud vs. t (uavg is the averaged data u), and
% a polynomial fitted to the averaged data. 
% 
% (The last figure 
% needs to be described better. We can actually entirely drop
% figure 2 and the associated processing; instead enough to 
% take a point from each of the two data sets uavg and ud ---
% will work on this later --- Ramprasad Potluri.)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

clear all, close all, clc;
x = dlmread('q10.log');
%x = y';

%[numrows,numcols] = size(x);

%if numcols/2 > floor(numcols/2)
%  x = x(:,1:numcols-1);
%  numcols = numcols-1;
%end

N =16;
numcols = N*2000;

% The PI controller forces Vr_hat = ud in steady state. Hence, we
% can use ud instead of Vr_hat. 

ud = x(1,1:2:numcols-1)/100; 
% Vector of voltages desired to be actually applied across the
% armature by the H-bridge.

u = x(1,2:2:numcols)/100;   
% Vector of voltages input to the H-bridge.

Ts = 0.002; % sampling interval
t = 0:Ts:Ts*(numcols/2-1);

% Plot of ud and u versus time. Data generated by the PI
% controller.
figure; grid; hold;
plot(t,ud); plot(t,u,'r');
xlabel('time  [s]'); ylabel('Voltages  [V]');
title('Raw (unprocessed) data given out from the main-prog-exp9.c');
legend ('u_d','u');

% Vr_hat = ud only in steady state. So(?), we have to remove the
% data of transient response of the system from the data points. 

% The FOR loop below removes the noise from the data generated
% by main-prog-exp9.c. The removal of noise is done by averaging
% each of the steps of the staircase signal. To perform this
% averaging, the transient response is neglected by excluding the
% transient points from the averaging. 

for k = 1:N
  usum = 0;
  % this loop sums the data of each step leaving some initial and
  % final points. 
  for a = 100:900
    i = (k-1)*1000 + a;
    usum = usum + u(i);
  end
			
  % this IF statement assigns a single value (THE AVERAGE) to the
  % whole step. 
  if (k==1)
    for b = 1:999
      j = (k-1)*1000 + b;
      uavg(j) = usum/800;
    end
  elseif (k<N)
    for b = 0:999
      j = (k-1)*1000 + b;
      uavg(j) = usum/800;
    end
  elseif(k==N)
    for b = 0:1000
      p = (k-1)*1000 + b;
      uavg(p) = usum/800;
    end
  end
end

% Plot ud and u (section wise average) versus time. 
% This is the data after going through the FOR loop.
figure; grid; hold;
plot(t,ud); plot(t,uavg,'g');
xlabel('time  [s]'); ylabel('Voltages  [V]');
title('Filtered version of data given out by main-prog-exp9.c');
legend ('u_d','u');

% The following FOR loop extracts the value of each step from the
% data generated by the above FOR loop.

for k = 1:N
  i = (k-1)*1000 + 500;
  u_out(k) = ud(i);
  u_in(k) = uavg(i);
end

% Fit a polynomial to the above extracted data points.
disp('The coefficients a, b, c of a polynomial ax^2 + bx + c');
disp('that is fitted to the above data are in the vector p as');
disp('p = [a,b,c]');
p = polyfit(u_out,u_in,2)

% NOTE: In main-prog.c, the polynomial to write is
% u = a*uc^2 + b*uc + c, where uc is the output of the controller. 

% Calculation of points of the best fitted curve for plotting the 
% polynomial
for k = 1:16
    u_fit(k) = p(3) + p(2)*u_out(k) + p(1)*u_out(k)*u_out(k);
end

% Plot of extracted data points and polynomial fitted to the curve
figure; grid; hold;
plot(u_out,u_in,':'); plot(u_out,u_fit,'r');
xlabel('input to compensator [V]'); ylabel('output of compensator [V]');
legend ('actual data','order 2 polynomial fit');
title('The inverse of input-output characteristics');
ylim ([0,9]);