% -----------------------------------------------------------------------------
% PlotYaw.m
% -----------------------------------------------------------------------------
%
%  This script plots the yaw angle progression of the hexapod, together with the
% horizontal plane trajectory of the robot, also depicting different leg phases
%
% The goal is to look at what the behavior arising from the current yaw turning
% controller is. For this purpose, it plots each stance phase colored
% differently. Stance phases start by the placement of the virtual foot and 
% finish when the virtual SLIP leaves the ground
%
% -----------------------------------------------------------------------------
%
%  function PlotYaw( vars );
%
% -----------------------------------------------------------------------------
%  Input Arguments:
%
%    vars : Hexapod data variables as loaded by LoadSimSectData.m
%
% -----------------------------------------------------------------------------
% $Id: PlotYaw.m,v 1.1.1.1 2003/10/28 20:05:59 ulucs Exp $
% -----------------------------------------------------------------------------

function PlotYaw( vars );

touchdownindices = find( diff( vars.fv(:,1) ) );
clf;

for ind = 1:length(touchdownindices);

  tdindex = touchdownindices( ind ) + 1;
  tdindexnext = touchdownindices( ind+1 ) + 1;
  liftoffindices = find( vars.slipmode(tdindex:end) <= 1 );
  loindex = tdindex + liftoffindices(1);

  rv = [ vars.r(:,1)-vars.fv(tdindex,1) vars.r(:,2)-vars.fv(tdindex,2) ...
		 vars.r(:,3)-vars.fv(tdindex,3) ];

  alphalineone = - vars.alphav( tdindex ) * rv( tdindex:loindex, 2 );
  alphalinetwo = - vars.alphav( tdindex ) * rv( loindex:tdindexnext, 2 );

%  subplot(3,1,1);
  plot( rv( tdindex:loindex, 1), rv( tdindex:loindex, 2), ...
        alphalineone, rv( tdindex:loindex, 2), ...
        rv( loindex:tdindexnext, 1), rv( loindex:tdindexnext, 2), ...
        alphalinetwo, rv( loindex:tdindexnext, 2) );
  axis tight;
  
%  subplot(3,1,2);
%  plot( vars.t( tdindex:loindex), vars.chart( tdindex:loindex) );
%  axis tight;
  
%  subplot(3,1,3);
%  plot( vars.t( tdindex:loindex), vars.slipmode( tdindex:loindex) );
%  axis tight;
  
  disp( sprintf('Touchdown is %i, liftoff is %i', tdindex, loindex ) );
  disp( 'Press any key for the next stride' );
  drawnow;
  pause;
end
