function [outP, outvc, outvs,vcon,vacc,an] = runcycle(am,vm,tc,T,P,vc)
% an attempt at implementing the EMC simple_tp
% this is an ugly roundoff trick required
% T==P will never evaluate to true for some reason...
mul=1e6;
if round(T*mul)/mul == round(P*mul)/mul
% done with this move
% remove current segment from queue
% get the next move
% return
sprintf('done with segment')
outP=T;
outvc=0;
outvs=0;
vcon=0;
vacc=0;
an=0;
return;
end
% the negative of the distance to go,
% reduced by the distance traveled during the next cycle if we have
% some vc right now and set vn=0
d= 0.5*tc*vc - (T-P);
if d>0.0
sprintf('error d>0.0 !, this should never happen !')
else
% some magic calculations ?
% the units of d actually change here
% above it is a distance
% here it is a time squared
d = 0.25 * (tc^2) - (2*d)/am;
% this is the calculation of the suggested velocity
vn = -0.5*am*tc+am*sqrt(d);
vs=vn; % store this vn in a variable vs = "v suggestion"
end
if (vn <= 0.0)
sprintf('error, vn<=0.0, this should never happen !')
else
% constrain velocity
if vn>vm
vn=vm;
vcon=vn; % store the constrained value for plotting
else
% if velocity was not constrained, hide the vcon plot
vcon=-1;
end
% calculate acceleration
an = (vn-vc)/tc;
% constrain acceleration
% and recalculate vn if neccessary
if (an > 0.0) && (an > am)
an= am;
vn=vc+an*tc;
vacc=vn; % store for plotting
vcon=-1; % if acceleration was constrained, hide the vcon plot
elseif (an<0.0) && (an < (-am))
an=- am;
vn=vc+an*tc;
vacc=vn; % store for plotting
vcon=-1; % if acceleration was constrained, hide the vcon plot
else
vacc=-1; % if acceleration was not constrined, hide the vacc plot
end
% update P
% this math is simple enough, even I 'get it'
P = P + (vn+vc)*0.5*tc;
end
vc = vn;
outP = P;
outvc = vc;
outvs = vs;