Point to Point Motion Profiles

1D motion profile generation

1D motion profile generation
Profiles (velocity, position) are generated from point to point, following designed curves (two sections: 3rd and const for velocity, and 4th and 1st for position):
0 - T1: v = vk_1 + 3.0 * dv * f^2 - 2.0 * dv * f^3, f = t/T1
T1 is calculated so that max(a) <= Amax (input accel)
dv = vk - vk_1
x = integrate of v
a = dv/dt
So, dx1max can be computed. If dx = xk - xk_1 < dx1max, decrease vk so that dx1max = dx, in this case profiles include only one section and T1 must be recalculated and T2 = 0.0, else dx1 = dx1max and dx2 = dx - dx1, in this case T1 is already set and T2 = dx2/dv.
One point (Pk) is defined with: vk and xk, and Amax is the max accel from Pk_1 to Pk. One more thing, the const section from Pn_1 to Pn (the last point in the profile) is added up to that from Pn_2 to Pn_1