CSP, MoveAbsolute and Jerk softmotion discuss

[feecat]

Hi

I'm coding with codesys a long time, i had use it to make some industrial controller, fieldbus adapter, cnc machine etc. I think i already known some principle about logical and softmotion function block.

But when i know more, i think the motion planning calc is very difficutily for me. For example, the real machine need s-curve acceleration or jerk planning, Normally just use function block like MC_MoveAbsolute and select velocity ramp mode in codesys, But i dont known how it calc position and speed in jerk motion. For now i can only realize trapezpid velocity ramp.
Then i had digger some information about motion planning... And i cannot found any plc core code about softmotion, they have a consensus that hide sourcecode. The closest is a paper about 4th order real-time trajectory generation function but no lucky to get sourcecode. Also no lucky in plcopen-motion public document.

Another way is fake a axis, due to define AXIS_REF_SM3 and make some patch like following, it bypass softmotion license and can be use MC_MoveAbsolute with full jerk funcion. Maybe i can do more work for it?

Please post any idea about this, Thanks.

Axis1:AXIS_REF_SM3;


Axis1.bCommunication:=TRUE;
Axis1.bRegulatorRealState:=TRUE;
Axis1.bDriveStartRealState:=TRUE;
IF Axis1.nAxisState=0 THEN
	Axis1.nAxisState:=3;
END_IF

Axis1.eRampType:=2;
Axis1.fSavePosition:=Axis1.fSetPosition;
Axis1.fCycleTimeSpent:=0;
Axis1.fTaskCycle:=0.001;

Some helped link:
1, codesys velocity ramp
2, Programming S-curve motion profiles
3, S-Curve acceleration?
4, ISG-MC_MoveAbsolute
5, TML-LIB_S7

[feecat]

Hi
I found a project called G2, They have a wiki show a 6th order planning, And it have core code at pnal_exec.cpp.
They use a pre define cycle time (not interrupt), and source is very cleanlly, so it was easy trans to codesys i think. and i had try write a little code for test, seems work perfect.

But g2 is a big project that i think, obviously i need more time to read source code.

[feecat]

Hi
According this post at duet3d forum, G2 use bezier curve to smooth trapezoid and it cannot control jerk in a single move. It was different with codesys planning, im not sure is this good for actual use.
Also there have an already exists discuss: Configurable maximum acceleration?

[feecat]

Hi
I have found some interesting code at 7Dof_Robot_Controller/7Dof_Elmo_Xenomai/src/mc_moveabsolute.cpp, It seems like sin2 velocity ramp in codesys. also it was easy to transformer to struct text.
I think maybe it not suitable for robot movemont, because it not easy to planning continue move.
Anyway it seems good begin for my target.

[feecat]

Hi
bad news

I'm try write some code same as g2 6th order planning, i found it base the trapezoid curve and pre-calc head_time, body_time and tail_time, But when we use it in cycle task, it will take a deadzone in two cycle, maybe 0.1mm or 1mm its defined by acceleration, max velocity and cycle time.

But it not happen in oscat.sramp( use sqrt) or sin2, they use last length to calc actual position.

It is more difficulty than i think.

[feecat]

Hi
I have reading a long time for tinyg and g2 source code, i have not figout jerk move but just change it to acceleration move, just like marlin.
For example, we have a move from 0 to 51.1111, maxvel = 50 and maxacc = 50, so we got head_length=25.0, body_length=1.1111, tail_length=25.0.
In g2 if you want make a move, it will divided segment_time to make sure every move_length is right( no deadzone). but it will take a unexpected jerk when into and out body. I dont have hardware to test it but in codesys i have make a trace:

In this example we got body_length = 1.1111 and body_time = 0.022222, it small than one cycle. force put it in any length it will take a unexpected jerk.
So i try move it to a trapezoid in tail_length, make deadzone length homogenic in tail move. But it also make a unexpected jerk at into and out tail_length. that because trapezoid move will superposition with tail move wave, so jerk also superposition.

And finally, i have use another bessel curve to superposition into tailmove, the unexpected jerk is gone. That i use bessel curve add into position, not control velocity, so we can got a very accurate length.

marlin dont have this problem because they use variable timer periods, it take a lot of work but without deadzone.
I don't have enough time and patience to read all the code, maybe my understanding will be wrong, please point it out.
Also, interrupting motion still make unexpected jerk, and it takes me a while to think how to process it.

userful links:
1, g2/plan_exec.cpp
2, marlin/stepper.cpp
3, At the Synthetos Booth

[feecat]

Hi
Here is my first public version, it just contain position calc part so i can focus it. To minimize jerk jitter, only in idle or body we can change target, it will take more move when acc is slow. You can download project file at here->Untitled65.zip

And leijian001/plcopen_motion_control Show some code that according Trajectory_Planning_for_Automatic_Machines_and_Robots.pdf, it explains trajectory planning very well, but a bit difficult for me to read.