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;