%
% PURPOSE: Plot i versus U for the two thruster curves from the Ody sim.
% AUTHOR: Rob McEwen
% DATE: 98/12/4
%
%
%   /*------------------------------------------------------------------*
%     The old model uses a drag power that is quadratic in desired speed,
%     which contradicts hydrodynamic theory. This model uses a cubic power
%     law that combines the fudge into effective C_d.
%
%                      I_des = 1/2 rho U^3 Cd A / V_batt
%
%     V_batt ~ 47.5 under load, A ~ 0.25 m^2, rho = 1025, combine all
%     constants yields 2.6978
%    *------------------------------------------------------------------*/
%

  units

  clear U Xuabu LHS

  V_batt  = 47.5;      %Volts
  Cdf     = .21;       %Forward drag coeff; unitless
  rho     = 1025;      %Density of Seawater; kg/m^3
  Af      = .25;       %Frontal area; m^2
  Kt      = .456;      %torque constant (in Nm/A) - inc gearbox */
  Eta	  = 0.80;      %propellor efficiency = KT/KE */
  Pit     = 0.124;     %prop pitch: advance/rad rotation (m/rad)*/
  Area    = 0.28;      %water jet (duct) projected area (in m2) */

%
% Computed constants:
%
  Xuabu   = -.5*rho*Cdf*Af;
  Xuabu   = -9.1;
  U       = [0:500]'/500*(1.75-1.25) + 1.25; % Speed vector - indep axis; m/s
  U       = [0:500]'/500*5 -2.5; % Speed vector - indep axis; [-2.5, 2.5] m/s

%
% thruster_current  = 2.6978 * sensor(EFFECTIVE_DRAG_COEFF) * pow(des_speed,3);
%
  thruster_current1 = .5*rho*Af*Cdf* U.^3/V_batt;
  limitIndex = find(abs(thruster_current1) > 5);
  thruster_current1(limitIndex) = 5*sign(thruster_current1(limitIndex));
%
% Their implementation.  I added the sign(U) to the discriminant to select
% the realistic root for U < 0.  With this addition, the *= sgn(des_speed) that
% they had now makes sense.
%
% The eqn. for i contains only an omega^2, and so it can't implicitly know
% the sign, hence the need for the sign() function.
%
% I then added the abs() function to the Cdf1 (also LHS) because this coeff. 
% of drag surely isn't valid for negative speeds.
%
  LHS = -Xuabu*(U.^2).*( 2.455 - 2.175*abs(U) +  0.75*(U.^2));

  des_omega = ( Area*rho*Eta*Pit*U + ...
                sign(U).*sqrt( (Area*rho*Eta*Pit*U).^2 + ...
                4.*LHS*Area*rho*Eta*Eta*Pit*Pit ) ) / ...
                (2*Area*rho*(Eta*Pit)^2);

  thruster_current2 = (.5*rho*Area*(Eta*Pit)^3.*des_omega.^2 - ...
                            .5*Area*rho*Eta*Pit*U.^2)/Kt;
  %/* make sign same as desired speed */
  thruster_current2 = thruster_current2.*sign(U);
  limitIndex = find(abs(thruster_current2) > 5);
  thruster_current2(limitIndex) = 5*sign(thruster_current2(limitIndex));


       %Cdf1 = ( 2.455 - 2.175*des_speed + 0.75*pow(des_speed,2.) );
	Cdf1 = ( 2.455 - 2.175*abs(U) +  0.75*(U.^2));
        Kthr = ( 1. + sqrt( 1. - 4*Xuabu.*Cdf1/rho/Area) )/Eta/Pit/2;
	des_omega3 = Kthr .* U;
        thruster_current3 = .5 * rho * Area * Eta * Pit * ...
                           ( (Eta*Pit*des_omega3).^2 - U.^2 )/Kt;
        thruster_current3 = thruster_current3.*sign(U);
        limitIndex = find(abs(thruster_current3) > 5);
        thruster_current3(limitIndex) = 5*sign(thruster_current3(limitIndex));
          

  caseName = 'QNX';
  str = [', Case:',caseName,' RSM ',myDate];
  PaperPosn = [.5 .5 3.75 2.5];
  FigurePosn = [5 30 815 760];
  i = 0;

  i=i+1;
  figure(i)
  clf
  set(gcf,'position',FigurePosn)
  subplot(2,1,1)
    plot(U, [thruster_current2 thruster_current3 thruster_current1],...
         'linewidth',2);
    grid on
    legend('Theory','Simplified Theory','Cubic Fix',0)
    ylabel('Amperes ')
    xlabel('Speed, Meters/Second')
    title(['Thruster Current Versus Speed',str]);
  subplot(2,1,2)
%   plot(U, [des_omega des_omega3 ], 'linewidth',2);
    plotyy(U, [des_omega des_omega3 ], U, [des_omega des_omega3 ]/rpm2rps);
    grid on
    legend('Theory','Simplified Theory',0)
    ylabel('Rad/Sec ')
    xlabel('Speed, Meters/Second')
    title(['Propeller Speed Versus Speed',str]);

return

  nameStr = 'thrCompPlt';
  doit = input(' Hardcopy? y => Create .eps, Y => Also send to Tahoe ','s');
  if(doit == 'y' | doit == 'Y')
    eval(['print -f1 -depsc ',nameStr,'_1'])
    if( doit == 'Y' )
      eval(['!lp -opostscript -onb -dtahoe ',nameStr,'_1.eps'])
    end
  end


