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: 
v1a : ARRAY [0..2] OFLREAL;
b : LREAL;
v1c : ARRAY [0..2] OF LREAL;

Output:

dot_funcmyCalculateAngle : LREAL;
Info

Type: Function

Responsible: Magnus


Simple helper function for 24_7 Forward Kinematics. Concept taken from wikipedia. Simply multiplies x1 * x2 + y1 * y2 + z1 * z2 = dot product.Using the law of cosine this function returns the angle between two sides of the triangle formed by the lengths of the sides. Used with 24_7 SpeedCalculation, which uses this angle to then find the ratio of movementspeed for each motor.


Code Block
languageactionscript3
titleCode Overview
linenumberstrue
FUNCTION dot_funcmyCalculateAngle : LREAL (* takes 3 sides of a triangle, returns angle *)
VAR_INPUT
	v1a : ARRAY [0..2] OFLREAL;
	b : LREAL;
	v2c : ARRAY [0..2] OF LREAL;
END_VAR
VAR
	resultnum : LREAL;
	den := 0LREAL;
	iangle : INTLREAL;
END_VAR


FOR i:=0 TO 2 DO
	result := result + v1[i] * v2[i];
END_FOR
dot_func := resultnum := (a*a + b*b - c*c);
den := (2*a*b);

IF b = 0 OR a = 0 THEN
	RETURN; (*Da funker det ikke*)
ELSE
	angle := num/den; // Tester nå med å bare returnere vinkelen. 
	IF angle > 1 THEN
		angle := 1;
	END_IF
	IF angle < -1 THEN
		angle := -1;
	END_IF
	angle := ACOS(angle);
END_IF

myCalculateAngle := angle;//*(180/3.141595);