Industrielle styresystemer - Wiki
You are viewing an old version of this page. View the current version.
Compare with Current View Page History
« Previous Version 4 Next »
v1 : ARRAY [0..2] OF LREAL;v1 : ARRAY [0..2] OF LREAL;
Output:
cross : ARRAY [0..2] OF LREAL;
Type: Function
Responsible: Magnus
Simple helper function for 24_7 Forward Kinematics. Concept taken from wikipedia.
FUNCTION cross : ARRAY [0..2] OF LREAL VAR_INPUT v1 : ARRAY [0..2] OF LREAL; v2 : ARRAY [0..2] OF LREAL; END_VAR VAR a1 : LREAL := v1[0]; a2 : LREAL := v1[1]; a3 : LREAL := v1[2]; b1 : LREAL := v2[0]; b2 : LREAL := v2[1]; b3 : LREAL := v2[2]; tempVec : ARRAY [0..2] OF LREAL; END_VAR tempVec[0] := (a2*b3-a3*b2); tempVec[1] := (a3*b1-a1*b3); tempVec[2] := (a1*b2-a2*b1); cross := tempVec;