% This script should give you some idea of how to use
% the other files in this directory.
% Katie Byl, 1/21/2010. ECE594D, Winter 2010. (UCSB)

format compact % for a single-spaced MATLAB command window

% (i.e., 3 degrees) You can reset it with a command like
% this, however, if you wish:
cg_gam_slope(3.5*pi/180);  % This sets the slope to 3.5 degrees

% Picking any initial condition that exists on the phase plot
% to find the next post-collision state.  To build the 
% Jacobian that characterizes the step-to-step transitions
% near the fixed point, you will need to START WITH THE
% POST-COLLISION FIXED POINT.  
Xfixed_4degrees = [-0.368803155919692
   0.229176815760146
  -0.216987420024786
  -1.140474944190978];

% Simulate several steps, and then animate the motion:
Xlast = Xfixed_4degrees + [.02;-.02;0;0]; % add noise...
tlast = 0;    % ADD this to time, to keep time monotonic
t_animation = [];  % time for animation frames
x_animation = [];  % states for animation frames
LF_animation = [];  % track where left foot is stance foot
x_impacts = [Xlast]; % a growing list of post-impact states
isLEFTfoot = false;  % alternate feet (colors) for animation
for n=1:10
    [Xlast,tani,xani] = cg_step(Xlast); % simulate one step
    x_impacts = [x_impacts, Xlast];  % append post-collision state
    t_animation = [t_animation; tlast+tani]; % time for animation
    tlast = max(t_animation)+1e-6; % to add to time (above)
    LF_animation = [LF_animation; isLEFTfoot+0*tani]; % (leg colors)
    x_animation = [x_animation; xani]; % x states for animation
    isLEFTfoot = ~isLEFTfoot; % just swaps animation colors of legs
end

% The variable below contains a list of post-collision states. 
% (Do they appear to converge to a fixed point over time?)
fprintf('Here is a list of the POST-COLLISION states:\n')
x_impacts   % with no ";" at end, simply displays variable value(s)
fprintf('You can alter the script to perform more steps\n')
fprintf('to estimate the fixed point (if any).\n')

% Now, use your new lists of states over time to do
% an ANIMATION of the walker:
figure(1)  % selection figure for the animation
cg_animate(t_animation,x_animation,LF_animation)
    