%--------------------------------------------------------------------------
%
% PURPOSE: Plot Tailcone Actuation Relation
% AUTHOR:  Rob McEwen
% DATE:    99/3/29
%
%--------------------------------------------------------------------------
%
  clear all
%
% Units and Fundamental Constants:
  in2m = .0254;             % Inches to meters.
  d2r  = pi/180;            % Degrees to radians
  r2d  = 180/pi;            % Radians to degrees
  rho  = 1028;              % Density of sea water at 0 Deg C; kg/m^3
  s2kg = 14.59390;          % Slugs to kg
  ft2m = .3048;             % Feet to meters
  sft2kgm2 = 1.355818;      % Slug-ft^2 to kg-m^2
  lbf2n    = 4.448;         % Pounds force to Newtons
  kph2mps  = 1852/3600;     % Knots per hour to meters per sec
  ftlbf2nm = 1.3558179;     % Foot-pounds force to Newton-meters.
  oi2nm    = .0070615516;   % Ounce-inches to Newton-meters.
%
  m2c= 200*12/in2m;         % Meters-to-counts
  aR = 2.50*in2m;           % Perp. dist from c.l. to rudder actuator base
  xRR = 11.5*in2m;          % See notes p. 13 and 17.
  rR1= 1.00*in2m;           % See notes p. 13 and 17.
  rR2= 2.50*in2m;           % See notes p. 13 and 17.
  Tmax = 10;                % Max rudder pivot torque, n-m
%
  deltaR = [0:500]'/500*2*pi/12 - pi/12;
%
% First, compute dist and force, given deltaR.
%
  dist0 = sqrt( (rR1 - xRR)^2 + (rR2 - aR)^2 );
  k1    = cos(deltaR)*rR1 - sin(deltaR)*rR2;
  k2    = sin(deltaR)*rR1 + cos(deltaR)*rR2;
  dist  = sqrt( (k1 - xRR).^2  + (k2 - aR).^2 );
  force = Tmax * dist./(k2*xRR - k1*aR);
%
% Approximate force assuming that aR=rR2 (see Act. notes, p.6), and gammaR
% is approximately = to pi/2-deltaR.
%
  forceA= Tmax/rR2./sin(pi/2-deltaR);
  forceA= Tmax/rR2./sin(pi/2+atan(rR1/rR2)-deltaR);
%  
% Now, go back the other way. That is, given dist, find deltaR.  See Actuator
% notes, p. 21, 99/6/3.
%
  b  = sqrt(rR1^2 + rR2^2);           % radius of circle generated by pivot pt.
  d1 = sqrt( aR^2 + xRR^2);
  delta0= acos( (d1^2 + b^2 - dist0^2)/(2*d1*b) );
  delta = acos( (d1^2 + b^2 - dist.^2)./(2*d1*b) );

  norm(deltaR - delta + delta0)


  figure(1)
  clf
  plot(deltaR, m2c*[(dist - dist0) b*deltaR  ] )
  xlabel(['radians; dist0 = ', num2str(dist0)])
  ylabel('Stepper Motor Counts')
  grid on

  figure(2)
  clf
  plot(deltaR, [force forceA]/lbf2n)
  xlabel(['radians; dist0 = ', num2str(dist0)])
  ylabel('Pounds')
  grid on


return

  figure(2)
  clf
% plot(deltaR,[force forceR])

  Nunits1 = 20;
  Ax = plotyy(deltaR, force, deltaR, force/lbf2n);
  set(gcf,'CurrentAxes',Ax(1));
  grid on
  ylabel('Axial Actuator Force, Newtons')
  ylim1 = get(gca,'ylim');
  set(gca,'ytick',[ceil(ylim1(1)/10)*10:Nunits1:floor(ylim1(2)/10)*10])
  set(gcf,'CurrentAxes',Ax(2));
  ylabel('Axial Actuator Force, Pounds Force')
  ylim2 = ylim1/lbf2n;
  Nunits2 = Nunits1/lbf2n;
  set(gca,'ylim',ylim2);
%  set(gca,'ytick',[ceil(ylim2(1)/10)*10:Nunits2:floor(ylim2(2)/10)*10])

  xlabel(['Rudder Angle, Rad; Tangential Force = [N, lbs] ', ...
           num2str([forceT forceT/lbf2n])])

%
%  if exist('hardflag')==0, hardflag=0;end
  caseName = '25Nm';

if 0
  if hardflag==0,
    return
  elseif hardflag==1,
    optstr = ' -depsc';      %Encapsulated Postscript
  elseif hardflag==2,
    optstr = ' -djpeg90';    %JPEG
  elseif hardflag==3,
    optstr = ' -dtiff';      %TIFF
  else
    disp(' Hardflag must be 0, 1, 2 or 3');
    return
  end

end
  optstr = ' -depsc';      %Encapsulated Postscript

  nfig = 2;
  for i = 1:nfig
    eval(['print -f', num2str(i), optstr, ' tcfig',num2str(i), caseName])
  end
%  hardflag = 0;




  if 0


  rP2Pr= sqrt( (rR1 - cos(deltaR)*xRR - sin(deltaR)*aR).^2 + ...
               (rR2 + sin(deltaR)*xRR - cos(deltaR)*aR).^2 );
  force = Tmax./((rR1*xRR + rR2*aR)*sin(deltaR) + (rR2*xRR - rR1*aR)*cos(deltaR));
  force = force.*rP2Pr;
  forceT= Tmax/sqrt(rR1^2 + rR2^2);     %Tangential force
  forceR= sqrt( force.^2 - forceT^2 );  %Radial force





%
% First Try; parallel configuration. 
%
  aR  = 3.75*in2m;          % Dist from c.l. to rudder actuator base
  xRR  = 12  *in2m;          % Dist from actuator base to gimbal attach point
%
  dist1   = aR*sqrt((sin(deltaR) + xRR/aR).^2 + (cos(deltaR) - 1).^2);
  dist2   = aR*(deltaR + xRR/aR);    % Linear version
  figure(1)
  clf
  plot(deltaR,[dist1 dist2])
  (dist2(501)-dist1(501))
  (dist2(501)-dist1(501))/dist1(501)
  grid on
  end