Trajectory planning

[AliEmad2002]

To take things step by step, the following notes assume 0 jerk. Jerk is to be added when these are done. Edited: θ is the tail-tail angle between the two vectors.

Moving G-code functions G0, G1 has only one speed related parameter, that is maximum feedrate. While in between line segments, speed should change according to the motion profile. Therefore, trajectory planning algorithm is needed.

Knowing that when wanting to move from point A to point B, given an initial speed Vi, final speed Vf and acceleration a, controller can easily apply the motion equation: V^2= Vi^2 + 2ax to get the speed at every segment of the line AB.

Vi is easy to get, as it is Vf of the previous movement. The thing we want to do is to find Vf of the movement AB.

Let Vb be the final speed at point B.

Let a point C be the next point to go to. This Vb is going to depend on the angle between lines AB, BC.

Notice that the more Vb is, the more the curvature error term ε is going to be.

We now need a function to estimate Vf at point, simplest assumption is: *Vf= Vmax cos(θ/2)**.

One other thing that should be considered, is that the output of the previous function can not be directly taken and used. It should be first checked such that machine can accelerate / decelerate to the following Vf, if it can’t, current Vf must be clamped.

[AliEmad2002]

Flowchart of the "plan_trajectory()" function:

[AliEmad2002]

Speed estimation is not right!

• Noted that two lines with angle 180 have speed = 0 between them.
• Also, when line is segmented for auto leveling, speed between segments is about 0.

Check the implementation.

[AliEmad2002]

Wrong θ was used. now corrected and running right.

[AliEmad2002]

• All of this turns to be non completed.

• The main problem so far is using just the total acceleration, and ignoring the acceleration in directions x and y.

• This is the plan to fix the problem, try it:

• _Making both deltat equal, still does not guarantee that both axis would be moving linearly with each-other... solve this.