Practical use case: Velocity sensor (PoC)

Now that we have the distance sensor available, we can extend its usage and implement a velocity sensor (radar). Just like the distance sensor it will work on parts but this time we will be measuring the speed at witch the part is moving relative to the sensor. If the part moves towards the sensor the speed will be positive. If the part is moving away from the sensor the speed will be negative.

The first step is to make the distance sensor a reusable block. e.g.

FUNCTION_BLOCK "DISTANCE_SENSOR"
VERSION:1.0
VAR_INPUT
	part : STRING;		// part name
	range : LREAL;		// detection range (mm)
	fov : LREAL;			// field of view (deg)
END_VAR
VAR_OUTPUT
	partDetected : BOOL;	// part is detected
	distance : LREAL;		// distance to the part
END_VAR
VAR
	loc : "ABS_LOC";
	plane : "PLANE_3D";
	normal : "VEC_3D";
	q : "VEC_3D";
	qp : "VEC_3D";
	p : "VEC_3D";
END_VAR
VAR_TEMP
	angle : REAL;
	prod : LREAL;
END_VAR
BEGIN
if SIM_TIME() = 0 then
	GET_ABS_LOC(#loc);    	    
	"GET_PLANE_FROM_LOC"(LOC:=#loc, PY:=100, PX:=100, PLN=>#plane);	
     "GET_PLANE_NORMAL_VEC"(PLN:=#plane, N=>#normal);
	"VEC_3D_NORM"(#normal); 
end_if;
#distance := GET_PART_APPEARANCE_DIST(PART:=#part, PTOP=>#q, PTOR=>#p);
if #distance >= 0 then       	
    "VEC_3D_SUB"(A:=#q, B:=#p, C=>#qp);
    "VEC_3D_NORM"(#qp);
    #prod := "VEC_3D_DOT_PROD"(A:=#normal, B:=#qp);
    if #prod >= -1 and #prod <= 1 then
    	   #angle := RAD_TO_DEG(ACOS(#prod));
    else
    	   #angle := 0;
	end_if;
	if #prod > 0 and #angle <= #fov / 2 then
    		#partDetected := #distance <= #range;
	    	if not #partDetected then
    			#distance := 0;
	    	end_if;
	else
    		#distance := 0;
	    	#partDetected := false;
    end_if;    	
else
   #distance := 0;
   #partDetected := false;
end_if;
END_FUNCTION_BLOCK

As usual save this to a UTF8 encoded .scl file and import it in Process Simulate.

Then what's left is to use the distance sensor for the purpose of the velocity sensor.

FUNCTION_BLOCK "MAIN"
VERSION:1.0
VAR_INPUT
	part : TX_OBJECT;				// part object
	range : LREAL := 1000000;		// detection range (mm)
	fov : LREAL := 80;			// field of view (deg)
END_VAR
VAR_OUTPUT
	velocity : LREAL;				// velocity (mm/s)
END_VAR
VAR
	lastDistance : LREAL;
	sensor : "DISTANCE_SENSOR";
	timer : TP_TIME;
END_VAR
VAR_TEMP
	distance : LREAL;
	partPresent : BOOL;
END_VAR
BEGIN
// call the distance sensor
#sensor(part:=#part.NAME, 
	range:=#range, fov:=#fov, 
	partDetected=>#partPresent, 
	distance=>#distance);
if #partPresent then
	// part is detected
	if #lastDistance <> #distance then
		// distance to part has changed so recalculate the velocity
		#velocity := (#lastDistance - #distance) / 	(UDINT_TO_REAL(LOGIC_UPDATE_RATE()) / 1000);
		#lastDistance := #distance;
	else
		// distance to part has not changed
		// introduce a timeout before resetting the velocity to zero
		#timer(IN:=true, PT:=T#10s);
		if not #timer.Q then 
			#timer(IN:=false);
			#velocity := 0;
		end_if;
	end_if;
else
	// part is not present so velocity cannot be determined
	#velocity := 0;
end_if;
END_FUNCTION_BLOCK

Paste this into the SCL editor and pick a part for the default value of TX_OBJECT. Also make sure the range and field of view serve your use case. Connect a signal to the velocity output and run the simulation. As the part moves the velocity changes. Once the part stops the velocity drops to zero.

Try it!

Note: In case of non-linear motion of the part, the reported velocity may not be accurate. This sensor assumes linear motion only!