startPoint : ARRAY [0..2] OF LREAL; endPoint : ARRAY [0..2] OF LREAL;
Output:
ARRAY [0..7] OF LREAL
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.
Code Overview
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