% Set up to run the simple system
% Run Step 1.m before using the Simulink model dimple_guidance.mdl
V=64       %  constant trim speed, ft per second
g=32.2      %  accel of gravity
xIC=[1000 1000 0] %  intitial condition
firstwp=2   % next must always start out at a value greater then 1 (so that next-1 is a valid waypoint)
%xwp=[0 100 200 300 1100]; % North waypoints (ft)
%ywp=[0 100 200 300 1100]; % East wapyoints (ft)
xwp=[1100 900 800 700 -1000  -1300 -1700 -2200 -2500 -1500 -1000 -2000 -1000    0  -500   -500]
ywp=[1100 900 800 700  1500   500  -500  -1500 -2500 -3000 -1500 -2000    0  -1000 -2000  1500]
hwp=[100  100 100 100  100    100   150   150   100   100   100   100   100    100  100   100 ]
%xwp=[1100 700 -1000    -2500]
%ywp=[1100 700 1500 -2500]
R=100; % Radius for determining how close you are to a waypoint
Kphi=-1/g    % Gain on Ccdot-Cdot
Kcdot=1/5    % Gain on Cc-C
% Now run the simulink model simple_guidance.mdl
% tThen run Step_2.m to plot the results