Input: 
a : LREAL;
b : LREAL;
c : LREAL;

Output:

myCalculateAngle : LREAL

Type: Function

Responsible: Magnus


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 Overview
FUNCTION myCalculateAngle : LREAL (* takes 3 sides of a triangle, returns angle *)
VAR_INPUT
	a : LREAL;
	b : LREAL;
	c : LREAL;
END_VAR
VAR
	num : LREAL;
	den : LREAL;
	angle : LREAL;
END_VAR


num := (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);


  • No labels

Industrielle styresystemer - NTNU Ålesund - Automasjon