Versions Compared

Old Version 1

changes.mady.by.user Kenneth Brenna

Saved on

compared with

New Version Current

changes.mady.by.user Magnus Antonsen

Saved on

Key

  • This line was added.
  • This line was removed.
  • Formatting was changed.
Input: 
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. Used in parallel with 24_7 AngleCalculation.Simple helper function for 24_7 Forward Kinematics. Concept taken from wikipedia. Simply multiplies x1 * x2 + y1 * y2 + z1 * z2 = dot product.


Code Block
languageactionscript3
titleCode Overview
linenumberstrue
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 2 DO
	result := result + v1[i] * v2[i];
END_FOR
dot_func := result;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