close all  % close all open plots
clear all  % clear all variables 

tc=0.001;  % cycle time
T=10.000;  % target
P=0.000;   % current progress
vc=0;      % current velocity
am=2;      % maximum acceleration
vm=3;      % maximum velocity


n=1;

while T ~= P    % call runcycle until we reach the end
    [P, vc, vs,vcon,vacc,an]=runcycle(am,vm,tc,T,P,vc);

    % store the instantaneous variables in vectors for plotting later
    Pr(n)=P;
    vcr(n)=vc;
    vsr(n)=vs;
    vconr(n)=vcon;
    vaccr(n)=vacc;
    anr(n)=an;
    n=n+1;
end
n=n-1; % there will be n elements 1..n in the recorded vectors



d=100;  % plot only every d:th point


cycl=[1:n];  % the x-axis for each plot

subplot(3,1,1)
plot(cycl(1:d:length(Pr)), Pr(1:d:length(Pr)),'.')
xlabel('cycles')
ylabel('Progress')
subplot(3,1,2)
plot(cycl(1:d:length(Pr)),vcr(1:d:length(Pr)),'o')

hold on
plot(cycl(1:d:length(Pr)),vsr(1:d:length(Pr)),'g.')
plot(cycl(1:d:length(Pr)),vconr(1:d:length(Pr)),'r.')
plot(cycl(1:d:length(Pr)),vaccr(1:d:length(Pr)),'m.')
axis([0 max(cycl) -0.1 1.1*max(vsr)]);

legend('v_{planned}','v_{suggest}','v_{vclamp}','v_{aclamp}')
parameters=sprintf('T=%f\nt_{c}=%f\nv_{max}=%f\na_{max}=%f',T,tc,vm,am);
text(100,1,parameters);
xlabel('cycles')
ylabel('velocity')
hold off

subplot(3,1,3)
plot(cycl(1:d:length(Pr)),anr(1:d:length(Pr)),'b.')
axis([0 max(cycl) -1.1*am 1.1*am])
xlabel('cycles')
ylabel('acceleration')

% independently check velocity and acceleration 
% by numerical differentiation of Pr
figure(2)
subplot(2,1,1)
plot(diff(Pr)./tc)
subplot(2,1,2)
plot(diff(diff(Pr))./(tc*tc))