Practical use case: Implementing MJV using a script

To continue the topic of custom motion profiles I'll try to implement the built-in MOVE_JOINT_TO_VALUE FB with a script.

The script will build on top of the TRAPEZOID_MOP FC showing you how to use it. As always scripts should be made available for the SCL script engine by importing them to the SCL Vault.

So let's begin:

To avoid naming conflicts with the built-in FB the script version should have a slightly different name: MOVE_JOINT_TO_VALUE_SCP

FUNCTION_BLOCK "MOVE_JOINT_TO_VALUE_SCP"
VERSION:1.0
VAR_INPUT
    joint : STRING;		// joint name
	target : LREAL;		// target value
	acc : LREAL;			// acceleration
	velocity : LREAL;		// speed
END_VAR
VAR_OUTPUT	
	value : LREAL;		// joint value
END_VAR
VAR
	t : LREAL;
	lastValue : LREAL;
	startTime : LREAL;
	trig : R_TRIG;
	move : BOOL;
	lastTarget : LREAL;
	multiplier : LREAL;
	pos : LREAL;
END_VAR
BEGIN
    #t := SIM_TIME();
    #t := #t / 1000; // ms to sec
    #trig(CLK:=#lastTarget <> #target or #t = 0); // new target set ?
    if #trig.Q then							//
	   	#startTime := #t;						// save the time
    		#lastValue := JDS(#joint);				// save the joint value
	    	#lastTarget := #target;				// save the target value
	    	if #target >= #lastValue then			// check the direction 
	    		#multiplier := 1;					// of movement
	    	else									//
	    		#multiplier := -1;				//
	    	end_if;								//
	    	#move := true;						// start the motion
	    	#trig(CLK:=false);					// reset the trigger
    end_if;
    if #move then
		// calculate the next joint value based on the motion profile
	    #pos := "TRAPEZOID_MOP"(VEL:=#velocity, ACC:=#acc,
    			DIST:=ABS(#target - #lastValue), 
  			ELAPSED_TIME:=ABS(#t - #startTime)) * #multiplier;
		// target value reached ?
   		if ABS(#target - #lastValue + #pos) > 0 then   						  	JUMP_JOINT(NAME:=#joint, VALUE:=#lastValue + #pos); // move the joint
	    	else
    			#move := false; // stop the motion
	    	end_if;
    end_if;
	#value := JDS(#joint); // get the joint value
END_FUNCTION_BLOCK

After import the main script calls this FB like any other:

FUNCTION_BLOCK "MAIN"
VERSION:1.0
VAR_INPUT
	target : LREAL;
	acc : LREAL;
	velocity : LREAL;
END_VAR
VAR_OUTPUT
	value : LREAL;
END_VAR
VAR
	mjv : "MOVE_JOINT_TO_VALUE_SCP";
END_VAR
BEGIN
	#mjv(joint:='j1',
	     acc:=#acc, 
	     velocity:=#velocity, 
	     target:=#target,
	     value=>#value);
END_FUNCTION_BLOCK

The FB moves the joint and returns the current joint value at the time of the call.

Wire the inputs and outputs to signals and observe the values in simulation panel.

It works for both revolute and prismatic joints.

Try it!

Note: The robot viewer reacts on time interval. The SCL script uses the LB update rate. Both should match in order to see the correct charts!