Industrielle styresystemer - Wiki
v : ARRAY [0..2] OF LREAL;
Output:
norm : LREAL;
Type: Function
Responsible: Magnus
Simple helper function for 24_7 Forward Kinematics. Concept taken from wikipedia. This function is the same as our inverse kinematic function.
FUNCTION norm : LREAL VAR_INPUT deltas : ARRAY [0..2] OF LREAL; END_VAR VAR i : INT; xD : LREAL := deltas[0]; yD : LREAL := deltas[1]; zD : LREAL := deltas[2]; END_VAR norm := SQRT((xD*xD)+(yD+yD)+(zD+zD));