%    HCTRL - script to plot heading control parameters in the current axes
%
%    B. A. Moran, 07-Nov-1997

% Copyright (C) 1997-1998, Massachussetts Institute of Technology
% Sea Grant Underwater Vehicles Laboratory
% $Revision: 1.2 $, $Date: 1998/02/10 02:45:43 $

yerr = modpi(data(:,m_yaw) - data(:,t_hdg));
ydot = data(:,m_yaw_rate) - data(:,t_hdg_rate);
plot( time, 180/pi*yerr*data(1,uo_control_kph), 'Color', 'r' );
line( time, 180/pi*ydot*data(1,uo_control_kdh), 'Color', [0 .5 0] );

if exist( 'hdg_integral' ) == 1
    line( time, 180/pi*data(:,hdg_integral), 'Color', [0 .75 .75] );
elseif exist( 'h_integral' ) == 1
    line( time, 180/pi*data(:,h_integral), 'Color', [0 .75 .75] );
end

line( time, 180/pi*data(:,c_rudder_angle), 'Color', 'b' );
zoom on

title( ['PID Heading (prop r, deriv g, rudr b)'] );
xlabel( 'elapsed time (s)' )
ylabel( 'control activity (deg)' )
