Industrielle styresystemer - Wiki
Industrielle styresystemer - Wiki
v1a : ARRAY [0..2] OFLREAL;
b : LREAL;
v1c : ARRAY [0..2] OF LREAL;
Output:
dot_funcmyCalculateAngle : LREAL;
Info |
---|
Type: Function Responsible: Magnus |
Simple helper function for 24_7 Forward Kinematics. Concept taken from wikipedia. Simply multiplies x1 * x2 + y1 * y2 + z1 * z2 = dot product.
Using the law of cosine this function returns the angle between two sides of the triangle formed by the lengths of the sides. Used with 24_7 SpeedCalculation, which uses this angle to then find the ratio of movementspeed for each motor.
Code Block | ||||||
---|---|---|---|---|---|---|
| ||||||
FUNCTION dot_funcmyCalculateAngle : LREAL (* takes 3 sides of a triangle, returns angle *) VAR_INPUT v1a : ARRAY [0..2] OFLREAL; b : LREAL; v2c : ARRAY [0..2] OF LREAL; END_VAR VAR resultnum : LREAL; den := 0LREAL; iangle : INTLREAL; END_VAR FOR i:=0 TO 2 DO result := result + v1[i] * v2[i]; END_FOR dot_func := resultnum := (a*a + b*b - c*c); den := (2*a*b); IF b = 0 OR a = 0 THEN RETURN; (*Da funker det ikke*) ELSE angle := num/den; // Tester nå med å bare returnere vinkelen. IF angle > 1 THEN angle := 1; END_IF IF angle < -1 THEN angle := -1; END_IF angle := ACOS(angle); END_IF myCalculateAngle := angle;//*(180/3.141595); |