Peter Nachtwey
Member
I worked out a simple filter for calculating velocity. The advantage is that one gets some filtering with little or no phase delay. This method isn't perfect just better than some other options. Low pass filters are great but they introduce phase lag that is unacceptable in dynamic situations. I have mentioned this technique before where one does a least squares fit for a second order polynomial s:=a+b*t+c*t^2 to the last 10 encoder readings. I chose 10 is arbitrary but most people liked 10. I can then take the derivative of the position function to get v:=b+2*c*t. The current encoder reading is always time 0 older readings are and negative times but we only care about the current time. If t is set to 0 then v=b. Simple. This means that only the coefficient b needs to be calaculated and it is just a weight average of the last 10 readings. I wrote this in SCL but one can probably implement this in a Rockwell AOI too or even a compute block.
Code:
(* CALCULATE VELOCITY *)
(* 090606 *)
// The current velocity is calculated by using an weighted average of
// the last 10 readings. The coefficients were derived by fitting a
// second order equation to the last 10 points and the taking the derivative
// at the last or current position. This method provide a filtered velocity
// with little or no lag or phased delay due to filtering.
// This function was tested using a sine function to generate position that were
// used as NewPos parameters. The resulting velocity should be close to the
// velocity calculated by taking the derivative of sine function. This
// filter isn't perfect, just better than using an low pass filter when phase
// delays are critical.
FUNCTION_BLOCK fbCalcVel
TITLE='fbCalcVel'
VAR_INPUT
NewPos: REAL; // the current position
DeltaT: REAL; // assumed to be fixed
END_VAR
VAR
arPos: ARRAY[0..9] OF REAL;
OldDeltaT: REAL:=0;
InvDeltaT: REAL;
END_VAR
VAR_TEMP
I: INT;
END_VAR
VAR_OUTPUT
Velocity: REAL;
END_VAR
IF OldDeltaT<>DeltaT THEN // check for change
OldDeltaT:=DeltaT;
InvDeltaT:=1.0/DeltaT; // for speed, could use updates per second instead
FOR I:=0 TO 9 BY 1 DO
arPos[I]:=NewPos; // initialize position history, assumed stopped
END_FOR;
END_IF;
arPos[9]:=arPos[8]; // crude but faster than circular queue
arPos[8]:=arPos[7];
arPos[7]:=arPos[6];
arPos[6]:=arPos[5];
arPos[5]:=arPos[4];
arPos[4]:=arPos[3];
arPos[3]:=arPos[2];
arPos[2]:=arPos[1];
arPos[1]:=arPos[0];
arPos[0]:=NewPos;
Velocity:=(0.25909090909090909091*arPos[0]
+0.11060606060606060606*arPos[1]
-0.37878787878787878788e-2*arPos[2]
-0.84090909090909090909e-1*arPos[3]
-0.13030303030303030303*arPos[4]
-0.14242424242424242424*arPos[5]
-0.12045454545454545455*arPos[6]
-0.64393939393939393939e-1*arPos[7]
+0.25757575757575757576e-1*arPos[8]
+0.15000000000000000000*arPos[9]
)*InvDeltaT;
END_FUNCTION_BLOCK