%
% PURPOSE: Plot Increasing Square wave data for all 3 cases.
% AUTHOR:  Rob McEwen
% DATE:  98/11/30
% MODS: 
% 98/11/19:
% 1) Multiplied V2lbs by 2 to account for Gary's voltage divider.
% 2) The conditioning amp zero is now at 2.5 volts, or 2048 A/D counts.
% Full scale is 0 to 5 volts. Previously, the cond. amp output range 
% was 106.38 lbs/Volt, centered on about -300 mV.

% 
% nameStr = input(' Data File Name: ','s')
%
% Load the data
% if(nameStr) 
%   eval(nameStr)
%   nameStrLast = nameStr;
% else
%   nameStr = nameStrLast;
% end
% Add lables below so that they match the order of the columns in asciiData.
%
% cnts2V = 5/2^11;           %Datel Conversion from AD counts to volts.
  cnts2V = 5/2^12;           %B&B Conversion from AD counts to volts.
  V2lbs  = 1/.00940*2;       %Conversion from Volts to lbs, from cal. data.
  lbs2N  = 4.448;            %Newtons per pound.
%
  c2N    = cnts2V*V2lbs*lbs2N;
  v2N    = V2lbs*lbs2N;
  MAX_CMD_THRUST         = 1245; %/* Maxiumum thrust value sent to motor  */
  MAX_MOTOR_TORQUE       =   22; %/* Max motor torque in Newton.metres*/
  VEL_PROP_GAIN      = 300;
  VEL_DERIV_GAIN     = 4000;
  VEL_POWER_GAIN     = 57;
  dynamicModel_oneOnK4 = 2400;
%
  thr2tq = MAX_MOTOR_TORQUE/MAX_CMD_THRUST;

  lbls = { 'time'
           'ADstatus'
           'ADvalue'
           'vel'
           'temp'
           'thrust'
         };
  sz = size(lbls);
  i = 1;
  while( i <= sz(1) ),
    eval([ char(lbls(i))  ' = '  num2str(i) ';']);
    i = i + 1;
  end

  thrMax = 800;
  rpmMax = 1500;

  if (exist('omdVEL10') == 0) 
     [omdVEL10 omDotVEL10 TqVEL10] = ...
     velocityMode(dataVEL10(:,vel),dataVEL10(:,thrust));
  end

  if (exist('omdVEL40') == 0) 
     [omdVEL40 omDotVEL40 TqVEL40] = ...
     velocityMode(dataVEL40(:,vel),dataVEL40(:,thrust));
  end

  figure(4)
  plot(data(:,time),[sgaugeCUR sgaugeVEL10 sgaugeVEL40 data(:,thrust)])
  ylabel('Thrust, Newtons')
  xlabel([' Seconds.  10 Sec Noise = ', num2str(Noise), ' Newtons, rms.'])
  grid on
  title([myDate ' Bias: ', num2str(bias)])
  axis([130 145 -thrMax thrMax])

  figure(5)
  plot(data(:,time),[dataCUR(:,vel) dataVEL10(:,vel) dataVEL40(:,vel)])
  ylabel('Shaft Speed, RPM')
  xlabel([' Seconds.  10 Sec Noise = ', num2str(Noise), ' Newtons, rms.'])
  grid on
  title([myDate ' Bias: ', num2str(bias)])
  axis([130 145 -rpmMax rpmMax])

  figure(6)
  plot(data(:,time),[TqVEL10';...
                     dataVEL10(:,thrust)'*thr2tq;...
                     (omdVEL10 -  dataVEL10(:,vel))'/VEL_PROP_GAIN;...
                     omDotVEL10'/VEL_DERIV_GAIN]')
  ylabel('Total Cmd, Nonlin Term, P Term, D Term; Newton-meters')
  xlabel([' Seconds.  10 Sec Noise = ', num2str(Noise), ' Newtons, rms.'])
  grid on
  title([myDate ' VEL10 Case.  Bias: ', num2str(bias)])
  axis([130 145 -thrMax*thr2tq*1.2 thrMax*thr2tq*1.2])

  figure(7)
  plot(data(:,time),[dataVEL10(:,vel) omdVEL10])
  ylabel('Measured and Commanded Speed, RPM')
  xlabel([' Seconds.  10 Sec Noise = ', num2str(Noise), ' Newtons, rms.'])
  grid on
  title([myDate ' VEL10 Case.  Bias: ', num2str(bias)])
  axis([130 145 -rpmMax*1.2 rpmMax*1.2])

  figure(8)
  plot(data(:,time),[TqVEL40';...
                     dataVEL40(:,thrust)'*thr2tq;...
                     (omdVEL40 -  dataVEL40(:,vel))'/VEL_PROP_GAIN;...
                     omDotVEL40'/VEL_DERIV_GAIN]')
  ylabel('Total Cmd, Nonlin Term, P Term, D Term; Newton-meters')
  xlabel([' Seconds.  10 Sec Noise = ', num2str(Noise), ' Newtons, rms.'])
  grid on
  title([myDate ' VEL40 Case.  Bias: ', num2str(bias)])
  axis([130 145 -thrMax*thr2tq*1.2 thrMax*thr2tq*1.2])

  figure(9)
  plot(data(:,time),[dataVEL40(:,vel) omdVEL40])
  ylabel('Measured and Commanded Speed, RPM')
  xlabel([' Seconds.  10 Sec Noise = ', num2str(Noise), ' Newtons, rms.'])
  grid on
  title([myDate ' VEL40 Case.  Bias: ', num2str(bias)])
  axis([130 145 -rpmMax*1.2 rpmMax*1.2])

