codesys 6轴机器人正解程序

cpp 复制代码

FUNCTION FUN_standard_DH_method : ARRAY[0..3,0..3] OF LREAL
VAR_INPUT
	theta:LREAL;
	d:LREAL;
	a:LREAL;
	alpha:LREAL;
END_VAR
VAR
END_VAR

cpp 复制代码

FUN_standard_DH_method[0,0]:=COS(theta);
FUN_standard_DH_method[0,1]:=-SIN(theta) * COS(alpha);
FUN_standard_DH_method[0,2]:=SIN(theta) * SIN(alpha);
FUN_standard_DH_method[0,3]:=a * COS(theta);

FUN_standard_DH_method[1,0]:=SIN(theta);
FUN_standard_DH_method[1,1]:=COS(theta) * COS(alpha);
FUN_standard_DH_method[1,2]:=-COS(theta) * SIN(alpha);
FUN_standard_DH_method[1,3]:=a * SIN(theta);

FUN_standard_DH_method[2,0]:=0;
FUN_standard_DH_method[2,1]:=SIN(alpha);
FUN_standard_DH_method[2,2]:=COS(alpha);
FUN_standard_DH_method[2,3]:=d;

FUN_standard_DH_method[3,0]:=0;
FUN_standard_DH_method[3,1]:=0;
FUN_standard_DH_method[3,2]:=0;
FUN_standard_DH_method[3,3]:=1;

cpp 复制代码

FUNCTION FUN_matrix_44_MUL : ARRAY[0..3,0..3] OF LREAL
VAR_INPUT
	matrix1:ARRAY[0..3,0..3] OF LREAL;
	matrix2:ARRAY[0..3,0..3] OF LREAL;
END_VAR
VAR
	i:INT;
	j:INT;
END_VAR

cpp 复制代码

FOR i:=0 TO 3 BY 1 DO
	FOR j:=0 TO 3 BY 1 DO
		FUN_matrix_44_MUL[i,j]:=matrix1[i,0] * matrix2[0,j] +
								matrix1[i,1] * matrix2[1,j] +
								matrix1[i,2] * matrix2[2,j] +
								matrix1[i,3] * matrix2[3,j];
		
	END_FOR
END_FOR

cpp 复制代码

VAR_GLOBAL
	theta:ARRAY[1..6] OF LREAL;
	d:ARRAY[1..6] OF LREAL;
	a:ARRAY[1..6] OF LREAL;
	alpha:ARRAY[1..6] OF LREAL;
	offset:ARRAY[1..6] OF LREAL;
	
	matrix:ARRAY [1..6] OF ARRAY [0..3,0..3] OF LREAL;
	endMatrix:ARRAY [0..3,0..3] OF LREAL;
END_VAR

cpp 复制代码

PROGRAM PLC_PRG
VAR	
	i:INT;
	x:LREAL;
	y:LREAL;
	z:LREAL;
END_VAR

cpp 复制代码

P_axisGroup();

theta[1]:=0+SM_Drive_Virtual_1.fActPosition/180*SMC_PI;
theta[2]:=SMC_PI/2+SM_Drive_Virtual_2.fActPosition/180*SMC_PI;
theta[3]:=0+SM_Drive_Virtual_3.fActPosition/180*SMC_PI;
theta[4]:=0+SM_Drive_Virtual_4.fActPosition/180*SMC_PI;
theta[5]:=0+SM_Drive_Virtual_5.fActPosition/180*SMC_PI;
theta[6]:=0+SM_Drive_Virtual_6.fActPosition/180*SMC_PI;

alpha[1]:=SMC_PI/2;
alpha[2]:=0;
alpha[3]:=SMC_PI/2;
alpha[4]:=SMC_PI/2;
alpha[5]:=-SMC_PI/2;
alpha[6]:=0;

d[1]:=0;
d[2]:=0;
d[3]:=0;
d[4]:=0;
d[5]:=0;
d[6]:=0;

a[1]:=0;
a[2]:=100;
a[3]:=0;
a[4]:=0;
a[5]:=0;
a[6]:=0;

FOR i:=1 TO 6 BY 1 DO
	matrix[i]:=FUN_standard_DH_method(theta:=theta[i] ,d:=d[i] ,a:=a[i] ,alpha:=alpha[i]);
END_FOR

endMatrix:=matrix[1];
FOR i:=2 TO 6 BY 1 DO
	endMatrix:=FUN_matrix_44_MUL(endMatrix,matrix[i]);
END_FOR

x:=endMatrix[0,3];
y:=endMatrix[1,3];
z:=endMatrix[2,3];