Industrielle styresystemer - Wiki
Industrielle styresystemer - Wiki
startPoint : ARRAY [0..2] OF LREAL;
endPoint : ARRAY [0..2] OF LREAL;
Output:
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. Used in parallel with 24_7 AngleCalculation.
Code Block | ||||||
---|---|---|---|---|---|---|
| ||||||
FUNCTION calculate_speed : ARRAY [0..7] OF LREAL VAR_INPUT startPoint : ARRAY [0..2] OF LREAL; endPoint : ARRAY [0..2] OF LREAL; END_VAR VAR 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 7 DO IF V[i] > 0 THEN D[i] := 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 |