Industrielle styresystemer - Wiki
Industrielle styresystemer - Wiki
v1startPoint : ARRAY [0..2] OF LREAL;
v1endPoint : ARRAY [0..2] OF LREAL;
Output:
dot_func :ARRAY [0..7] OF LREAL;
Info |
---|
Type: Function Responsible: Magnus, Simen |
Calculates the speed of each motor using Simens calculations. The basic overview of how the calculations work can be found at 24_7 Simulation and Visualization.Simple helper function for 24_7 Forward Kinematics. Concept taken from wikipedia. Simply multiplies x1 * x2 + y1 * y2 + z1 * z2 = dot product.
Code Block | ||||||
---|---|---|---|---|---|---|
| ||||||
FUNCTION dotcalculate_funcspeed : ARRAY [0..7] OF LREAL VAR_INPUT v1startPoint : ARRAY [0..2] OF LREAL; v2endPoint : ARRAY [0..2] OF LREAL; END_VAR VAR result A : LREAL; B : ARRAY [0..7] OF LREAL; C : ARRAY [0..7] OF LREAL; V : ARRAY [0..7] OF LREAL; V2 : ARRAY [0..7] OF LREAL; D : ARRAY [0..7] OF LREAL; theMax : LREAL := 0; i : INT := 0; END_VAR // Returns calculated speed for all motors given a startPoint and endPoint A := myHelperForLinear(start:=startPoint, end:=endPoint); B := myInverseKinematicsOffsets(targetPosX := startPoint[0], targetPosY := startPoint[1], targetPosZ := startPoint[2]); C := myInverseKinematicsOffsets(targetPosX := endPoint[0], targetPosY := endPoint[1], targetPosZ := endPoint[2]); i := 0; FOR i := 0 TO 7 DO V[i] := -COS(myCalculateAngle(a:=A,b:=B[i],c:=C[i])); END_FOR theMax := 0; i := 0; FOR i := 0 TO 7 DO V2[i] := ABS(V[i]); END_FOR i := 0; FOR i := 0 TO 27 DO resultIF V[i] > 0 THEN D[i] := result + v1[i] * v2[i]; END_FOR dot_func := result;1; ELSE D[i] := -1; END_IF IF V2[i] > theMax THEN theMax := V2[i]; (*Normalizes the speeds*) END_IF END_FOR i := 0; FOR i := 0 TO 7 DO calculate_speed[i] := V[i];//(V2[i]/theMax) * D[i]; (* Multiplies by eigher 1 or -1 to set direction*) //Doesnt need this anymoreeeeeeee FINALY END_FOR |