24_7 SpeedCalculation

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