Moja wersja to by chyba na Arduino już nie poszła

////////////////////////////////////////////////////////////////////////////////
// xyz_GeometryCalculateAxis()
////////////////////////////////////////////////////////////////////////////////

void xyz_GeometryCalculateAxis(
U32 PulsesPer1000mm,
VectorSpeedType * VS
)
{
// Calculates (for single axis) its acc/const/dcc motion profile

if(VS->Distance != 0)
{
if( (VS->Acc != 0) && (VS->Dcc != 0))
{
VS->LineAcc.T = (VS->V-VS->Vstart) / VS->Acc;
VS->LineDcc.T = (VS->V-VS->Vstop) / VS->Dcc;


VS->LineAcc.S = VS->LineAcc.T * VS->Vstart + VS->Acc * VS->LineAcc.T * VS->LineAcc.T /2;
VS->LineDcc.S = VS->LineDcc.T * VS->Vstop + VS->Dcc * VS->LineDcc.T * VS->LineDcc.T /2;

if(VS->LineAcc.S + VS->LineDcc.S < VS->Distance)
{
VS->LineConst.S = VS->Distance - VS->LineAcc.S - VS->LineDcc.S;
VS->LineConst.T = (VS->LineConst.S / VS->V);

VS->LineAcc.PulseCnt = xyz_GeometryDist2Pulse(VS->LineAcc.S,PulsesPer1000mm);
VS->LineDcc.PulseCnt = xyz_GeometryDist2Pulse(VS->LineDcc.S,PulsesPer1000mm);
VS->LineConst.PulseCnt = xyz_GeometryDist2Pulse(VS->Distance,PulsesPer1000mm) - VS->LineAcc.PulseCnt - VS->LineDcc.PulseCnt;

assert( fabs(VS->V -( VS->Vstart + VS->Acc*VS->LineAcc.T)) < 0.001);

}
else
{
double WA,WB,WC;

VS->LineConst.T = 0;
VS->LineConst.S = 0;
VS->LineConst.PulseCnt = 0;

WA = VS->Acc/2 + VS->Dcc/2 *(VS->Acc/VS->Dcc)*(VS->Acc/VS->Dcc);
WB = VS->Vstart + (VS->Acc/VS->Dcc)*VS->Vstop;
WC = -(double)VS->Distance;


VS->LineAcc.T = (-WB+sqrt(WB*WB-4*WA*WC))/(2*WA);
VS->LineAcc.S = VS->Acc*VS->LineAcc.T*VS->LineAcc.T/2 + VS->LineAcc.T*VS->Vstart;
VS->LineAcc.PulseCnt = xyz_GeometryDist2Pulse(VS->LineAcc.S,PulsesPer1000mm);

VS->LineDcc.T = (VS->LineAcc.T*VS->Acc)/VS->Dcc;
VS->LineDcc.S = VS->Dcc*VS->LineDcc.T*VS->LineDcc.T/2 + VS->LineDcc.T*VS->Vstop;
VS->LineDcc.PulseCnt = xyz_GeometryDist2Pulse(VS->Distance,PulsesPer1000mm) - VS->LineAcc.PulseCnt;

VS->V = VS->Vstart + VS->Acc*VS->LineAcc.T;

}
}
else
{
VS->LineAcc.T = 0;
VS->LineAcc.S = 0;
VS->LineAcc.PulseCnt = 0;



VS->LineConst.T = (double)VS->Distance/VS->V;
VS->LineConst.S = VS->Distance;
VS->LineConst.PulseCnt = xyz_GeometryDist2Pulse(VS->Distance,PulsesPer1000mm);

VS->LineDcc.T = 0;
VS->LineDcc.S = 0;
VS->LineDcc.PulseCnt = 0;
}
}
else
{
VS->LineAcc.T = 0;
VS->LineAcc.S = 0;
VS->LineAcc.PulseCnt = 0;

VS->LineConst.T = 0;
VS->LineConst.S = 0;
VS->LineConst.PulseCnt = 0;

VS->LineDcc.T = 0;
VS->LineDcc.S = 0;
VS->LineDcc.PulseCnt = 0;

}
}