纯手工Codesys的6轴机械手控制程序记录

目录

一、概述

二、程序的架构

 3.1 截图

 3.2 架构介绍

 3.3 代码的展示

         3.3.1机械手正解

         3.3.2机械手逆解

         3.3.3主程序

         3.3.4运动主程序

         3.3.4.1直线子运动程序

         3.3.4.2阿基米德旋线+球面子运动程序

三、总结


一、概述

 

        一直想学习6轴机械手的运动学,一直没有空闲。趁着12月疫情,闭门研究6轴机械手的运动并且用codesys简单实现。

        机械手的控制我自己的理解是,机械手末端的位置和方向 -> 逆解出对应各关节的角度 ->控制关节旋转至目标角度。机械手的执行的运动品质,就转变为 机械手末端的位置和方向的大量的插补点,而且要考虑这些插补点逆解出的各关节的角度的运动趋势。特别是 各关节的运动趋势,因为运动趋势是最终关节电机如何运转和能不能运转的关键点。

        本文利用codesys实现和验证6轴机械手的正解和逆解和简易的6轴机械手的插补运动(直线、阿基米德旋线+球面)。其中直线运动子程序,做了简单的梯形加减速,是固定方向下的位置直线运动轨迹;而阿基米德旋线+球面则只做了位置插补控制,做了变方向和变位置的运动轨迹。

二、程序的架构

3.1 截图

纯手工Codesys的6轴机械手控制程序记录_第1张图片纯手工Codesys的6轴机械手控制程序记录_第2张图片

纯手工Codesys的6轴机械手控制程序记录_第3张图片

 3.2 架构介绍

程序主要分了3个task。

MainTask(100ms,优先级8) := 按钮和界面的触发和更新。

Motion_Task(50ms,优先级2) := 运动的插补运算。

Servo_Task(1ms,优先级1) := 关节伺服运动控制功能块。(虽然是虚拟轴)

 3.3 代码的展示

         3.3.1机械手正解


//DirectKinematics 机械手正解函数,作者:AlongWu


FUNCTION DirectKinematics : HandJointPos
VAR_INPUT
	

HJointAg			:HandJointAg;										//机械手关节数据

END_VAR
VAR
	
T_Htf_Arr			:ARRAY[0..5] OF ARRAY[0..3,0..3] OF LREAL;			//齐次矩阵数组
T_Arr				:ARRAY[0..5] OF ARRAY[0..3,0..3] OF LREAL;			//机械手关节映射矩阵

i					:INT;




END_VAR



//DH表格thet角赋值

GVL.DHTable.ItemArr[0].Thet  := HJointAg.J1;
GVL.DHTable.ItemArr[1].Thet  := HJointAg.J2;
GVL.DHTable.ItemArr[2].Thet  := HJointAg.J3;
GVL.DHTable.ItemArr[3].Thet  := HJointAg.J4;
GVL.DHTable.ItemArr[4].Thet  := HJointAg.J5;
GVL.DHTable.ItemArr[5].Thet  := HJointAg.J6;


//DH表格转齐次矩阵


FOR i:=0 TO 5 BY 1 DO 
	T_Htf_Arr[i] := HTF_MatrixSTD(GVL.DHTable.ItemArr[i]);
END_FOR

//右乘,获得全部关节的映射矩阵
T_Arr[0] := T_Htf_Arr[0];
T_Arr[1] :=Matrix44PointMul(T_Arr[0],T_Htf_Arr[1]);
T_Arr[2] :=Matrix44PointMul(T_Arr[1],T_Htf_Arr[2]);
T_Arr[3] :=Matrix44PointMul(T_Arr[2],T_Htf_Arr[3]);
T_Arr[4] :=Matrix44PointMul(T_Arr[3],T_Htf_Arr[4]);
T_Arr[5] :=Matrix44PointMul(T_Arr[4],T_Htf_Arr[5]);


//通过映射矩阵获取各关节的位置信息

DirectKinematics.J1 := RotationMatrix2pos(T_Arr[0]);
DirectKinematics.J2 := RotationMatrix2pos(T_Arr[1]);
DirectKinematics.J3 := RotationMatrix2pos(T_Arr[2]);
DirectKinematics.J4 := RotationMatrix2pos(T_Arr[3]);
DirectKinematics.J5 := RotationMatrix2pos(T_Arr[4]);
DirectKinematics.J6 := RotationMatrix2pos(T_Arr[5]);




         3.3.2机械手逆解

//机械手逆解 InverseKinematics  ,作者:AlongWu
FUNCTION InverseKinematics : HandJointResult

VAR CONSTANT
EmptyJointStru				:ARRAY[0..3] OF HandJointAg:=[															
															(J1Gap:=0,J2Gap:=0,J3Gap:=0,J4Gap:=0,J5Gap:=0,J6Gap:=0,J1:=0,J2:=0,J3:=0,J4:=0,J5:=0,J6:=0),
															(J1Gap:=0,J2Gap:=0,J3Gap:=0,J4Gap:=0,J5Gap:=0,J6Gap:=0,J1:=0,J2:=0,J3:=0,J4:=0,J5:=0,J6:=0),
															(J1Gap:=0,J2Gap:=0,J3Gap:=0,J4Gap:=0,J5Gap:=0,J6Gap:=0,J1:=0,J2:=0,J3:=0,J4:=0,J5:=0,J6:=0),
															(J1Gap:=0,J2Gap:=0,J3Gap:=0,J4Gap:=0,J5Gap:=0,J6Gap:=0,J1:=0,J2:=0,J3:=0,J4:=0,J5:=0,J6:=0)
														 ];

														 
IptAgMax					:LREAL:=500;			//插补角度间距允许值													
END_VAR


VAR_INPUT
	
KeyPosStru					:PosStru;					//目标坐标数据元
SortType					:INT;						//逆解结果的决策要求(0=总关节移动量最少,1=每个关节移动量不能大于"插补角度间距允许值")

END_VAR
VAR
	

i							:INT;
j							:INT;
//k							:INT;

T							:ARRAY[0..3,0..3] OF LREAL;							//目标坐标元的旋转矩阵
T_Arr						:ARRAY[0..5] OF ARRAY[0..3,0..3] OF LREAL;			//机械手关节映射矩阵

nx							:LREAL;
ny							:LREAL;
nz							:LREAL;
ox							:LREAL;
oy							:LREAL;
oz							:LREAL;
ax							:LREAL;
ay							:LREAL;
az							:LREAL;
px							:LREAL;
py							:LREAL;
pz							:LREAL;

tmp1						:LREAL;
tmp2						:LREAL;
tmp3						:LREAL;
tmp4						:LREAL;
tmp5						:LREAL;
tmp6						:LREAL;
d1							:LREAL;
d4							:LREAL;
d5							:LREAL;
d6							:LREAL;
a2							:LREAL;
a3							:LREAL;
sin1						:LREAL;
cos1						:LREAL;
sin2						:LREAL;
cos2						:LREAL;
sin3						:LREAL;
cos3						:LREAL;
sin4						:LREAL;
cos4						:LREAL;
sin5						:LREAL;
cos5						:LREAL;
sin6						:LREAL;
cos6						:LREAL;
th1							:LREAL;
th2							:LREAL;
th3							:LREAL;
th4							:LREAL;
th5							:LREAL;
th6							:LREAL;
				
m							:LREAL;
n							:LREAL;

c							:BOOL;

Thet1						:ARRAY[0..1] OF Thet1Result;				//逆解角度结果缓存

tmpJointStru				:ARRAY[0..3] OF HandJointAg;				//逆解结果缓存

tmpJointStruLen				:INT;

tmpJs						:HandJointAg;

CheckHandJointPos			:HandJointPos;

END_VAR



//isNull的标志复位
InverseKinematics.IsNull :=FALSE;


//目标坐标数据元转旋转矩阵
T :=PosStur2RotationMatrix(KeyPosStru);


//旋转矩阵的值定位
nx:=T[0,0];
ny:=T[1,0];
nz:=T[2,0];
ox:=T[0,1];
oy:=T[1,1];
oz:=T[2,1];
ax:=T[0,2];
ay:=T[1,2];
az:=T[2,2];
px:=T[0,3];
py:=T[1,3];
pz:=T[2,3];


//DH表格赋值
d6 := GVL.DHTable.ItemArr[5].ZGap;
d4 := GVL.DHTable.ItemArr[3].ZGap;
d1 := GVL.DHTable.ItemArr[0].ZGap;
d5 := GVL.DHTable.ItemArr[4].ZGap;

a2 := GVL.DHTable.ItemArr[1].Bridge;
a3 := GVL.DHTable.ItemArr[2].Bridge;


//开始计算


tmp1 := d6 * ay - py;
tmp2 := px - d6 * ax;
tmp1 := tmp1 * tmp1 + tmp2 * tmp2;

IF tmp1 = 0 THEN
	tmp2 :=0;
ELSE
	tmp2 := d4 / SQRT(tmp1);
END_IF
 
IF tmp2 > 1 OR tmp2 <-1 THEN
	//目标姿态不能实现
	InverseKinematics.IsNull :=TRUE;
	RETURN;
END_IF   

tmp4 := ACOS(tmp2) * 180 / GVL.PI;
tmp5 := MATH.Atan2(px - d6 * ax, d6 * ay - py) * 180 / GVL.PI;


//thet1的2个解
Thet1[0].thet1 := tmp5 - tmp4;
Thet1[1].thet1 := tmp5 + tmp4;

FOR i:=0 TO 1 BY 1 DO
	
	th1 := 	Thet1[i].thet1;
	sin1 := SinAg(th1);
	cos1 := CosAg(th1);
	
	//计算th6
	tmp1 := sin1*ox - cos1*oy;
	tmp2 := sin1*nx - cos1*ny;

	th6 := MATH.atan2(-tmp1,tmp2)*180 / GVL.PI;	
	sin6 := SinAg(th6);
	cos6 := CosAg(th6);
	
	Thet1[i].thet6 :=th6;
	
	
	//计算th5
	tmp1 := (sin1 * nx - cos1 * ny)*cos6-(sin1*ox-cos1*oy)*sin6;

	IF tmp1>1 OR tmp1<-1 THEN
		结束本次循环,进入下一个循环
		CONTINUE;
	END_IF 
              
    tmp2 := ASIN(tmp1) ;
    tmp3 := sin1 * ax - cos1 * ay;

	IF tmp3>1 OR tmp3<-1 THEN
		结束本次循环,进入下一个循环
		CONTINUE;
	END_IF 
            
    tmp4 := ACOS(tmp3) * 180 / Math.PI;

	IF tmp2>=0 THEN
		Thet1[i].thet5 := tmp4;
	ELSE		
	 	Thet1[i].thet5 := -tmp4;
	END_IF
              
    th5 := Thet1[i].thet5;
    sin5 := SinAg(th5);
    cos5 := CosAg(th5);

	//计算th3
  	m := d5 * ((cos1 * nx + sin1 * ny) * sin6 + (cos1 * ox + sin1 * oy) * cos6) - d6 * (cos1 * ax + sin1 * ay) + cos1 * px + sin1 * py;
    n := d5 * (nz * sin6 + oz * cos6) - d6 * az + pz - d1;

   	tmp1 := 2 * a2 * a3;
	
	IF tmp1 = 0 THEN
		tmp3 := 0;
	ELSE
		 tmp3 := (m * m + n * n - a2 * a2 - a3 * a3) / tmp1;
	END_IF
                  

    IF tmp3 > 1 OR tmp3 < -1 THEN
		//结束本次循环,进入下一个循环
		CONTINUE;
	END_IF
                  
   	tmp4 := ACOS(tmp3) * 180 / GVL.PI;

    Thet1[i].thet3r[0].thet3 := -tmp4;
    Thet1[i].thet3r[1].thet3 := tmp4;
	
	FOR j:=0 TO 1 BY 1 DO
		
		th3 := Thet1[i].thet3r[j].thet3;                  
       	cos3 := CosAg(th3);
        sin3 := SinAg(th3);
		
		//计算 th2
        tmp1 := n * (a2 + a3 * cos3) - m * a3 * sin3;
        tmp2 := m * (a2 + a3 * cos3) + n * a3 * sin3;

        th2 := (MATH.Atan2(tmp1, tmp2) * 180 / Math.PI);
                    
        Thet1[i].thet3r[j].thet2 := th2;
		
		//计算 th4
        tmp1 := (nz * cos6 - oz * sin6) * cos5 - az * sin5;
        tmp2 := ((cos1 * nx + sin1 * ny) * cos6 - (cos1 * ox + sin1 * oy) * sin6) * cos5 - (cos1 * ax + sin1 * ay) * sin5;

        tmp3 := MATH.Atan2(tmp1, tmp2) * 180 / Math.PI;

        th4 := tmp3 - th2 - th3;
        Thet1[i].thet3r[j].thet4 := th4;

	END_FOR

END_FOR


//角度超限检查并且 留下合规的解

//复位逆解结果缓存
tmpJointStru := EmptyJointStru;
tmpJointStruLen :=0;

FOR i:=0 TO 1 BY 1 DO
	FOR j:=0 TO 1 BY 1 DO
		c := LrealZooCmp(GVL.JointParamArr[1].maxAg,GVL.JointParamArr[1].minAg,Thet1[i].thet3r[j].thet2);
		IF c THEN
			c := LrealZooCmp(GVL.JointParamArr[2].maxAg,GVL.JointParamArr[2].minAg,Thet1[i].thet3r[j].thet3);
			IF c THEN
				c := LrealZooCmp(GVL.JointParamArr[3].maxAg,GVL.JointParamArr[3].minAg,Thet1[i].thet3r[j].thet4);
				IF c THEN
					c := LrealZooCmp(GVL.JointParamArr[4].maxAg,GVL.JointParamArr[4].minAg,Thet1[i].thet5);
					IF c THEN
						c := LrealZooCmp(GVL.JointParamArr[5].maxAg,GVL.JointParamArr[5].minAg,Thet1[i].thet6);
						IF c THEN
							tmpJointStru[tmpJointStruLen].J1 := Thet1[i].thet1;							
							tmpJointStru[tmpJointStruLen].J1Gap := Thet1[i].thet1 - GVL.LR_Hand_J1;
							
							tmpJointStru[tmpJointStruLen].J2 := Thet1[i].thet3r[j].thet2;							
							tmpJointStru[tmpJointStruLen].J2Gap := Thet1[i].thet3r[j].thet2 - GVL.LR_Hand_J2;
							
							tmpJointStru[tmpJointStruLen].J3 := Thet1[i].thet3r[j].thet3;							
							tmpJointStru[tmpJointStruLen].J3Gap := Thet1[i].thet3r[j].thet3 - GVL.LR_Hand_J3;
							
							tmpJointStru[tmpJointStruLen].J4 := Thet1[i].thet3r[j].thet4;						
							tmpJointStru[tmpJointStruLen].J4Gap := Thet1[i].thet3r[j].thet4 - GVL.LR_Hand_J4;
							
							tmpJointStru[tmpJointStruLen].J5 := Thet1[i].thet5;							
							tmpJointStru[tmpJointStruLen].J5Gap := Thet1[i].thet5 - GVL.LR_Hand_J5;
								
							tmpJointStru[tmpJointStruLen].J6 := Thet1[i].thet6;							
							tmpJointStru[tmpJointStruLen].J6Gap := Thet1[i].thet6 - GVL.LR_Hand_J6;
							
							tmpJointStruLen := tmpJointStruLen +1;
						END_IF
					END_IF
				END_IF
			END_IF
		END_IF
	END_FOR
END_FOR


//根据 SortType 挑选最优解并输出

IF tmpJointStruLen > 0 THEN
	
//先赋值第一个解

	tmpJs :=tmpJointStru[0];

	tmp1 := ABS(tmpJs.J1Gap)+ ABS(tmpJs.J2Gap)+ ABS(tmpJs.J3Gap)+ ABS(tmpJs.J4Gap)+ ABS(tmpJs.J5Gap)+ ABS(tmpJs.J6Gap);
	
	//正解 逆解的结果 ,获取各关节的位置
	CheckHandJointPos := DirectKinematics(tmpJs);
	
	//区域限制
	//各关节 不能低于 底面50
	IF CheckHandJointPos.J1.pz <50 OR  CheckHandJointPos.J2.pz <50 OR  CheckHandJointPos.J3.pz <50 OR  CheckHandJointPos.J4.pz <50 OR  CheckHandJointPos.J5.pz <50 OR  CheckHandJointPos.J6.pz <50 THEN
		//不符合条件,剔除
		InverseKinematics.IsNull := TRUE;
		
	ELSE
		//判断SortType
		
		IF SortType =1 THEN 
			
			IF ABS(tmpJs.J1Gap)>IptAgMax OR ABS(tmpJs.J2Gap)>IptAgMax OR ABS(tmpJs.J3Gap)>IptAgMax OR ABS(tmpJs.J4Gap)>IptAgMax OR ABS(tmpJs.J5Gap)>IptAgMax OR ABS(tmpJs.J6Gap)>IptAgMax  THEN
				//不符合条件,剔除
				InverseKinematics.IsNull := TRUE;
				
			ELSE
				//符合条件,赋值
				InverseKinematics.HandJoint := tmpJs;
				
			END_IF	
		ELSE
			//符合条件,赋值
			InverseKinematics.HandJoint := tmpJs;
		END_IF
	
	
	END_IF
	
	IF tmpJointStruLen > 1 THEN
		IF InverseKinematics.IsNull THEN
			tmp1:= 1000000;
		END_IF
		
		FOR i:=1 TO (tmpJointStruLen-1) BY 1 DO
			
			tmpJs :=tmpJointStru[i];
		
			tmp2 := ABS(tmpJs.J1Gap)+ ABS(tmpJs.J2Gap)+ ABS(tmpJs.J3Gap)+ ABS(tmpJs.J4Gap)+ ABS(tmpJs.J5Gap)+ ABS(tmpJs.J6Gap);
	
			//正解 逆解的结果 ,获取各关节的位置
			CheckHandJointPos := DirectKinematics(tmpJs);
			//区域限制
			//各关节 不能低于 底面50
			IF CheckHandJointPos.J1.pz <50 OR  CheckHandJointPos.J2.pz <50 OR  CheckHandJointPos.J3.pz <50 OR  CheckHandJointPos.J4.pz <50 OR  CheckHandJointPos.J5.pz <50 OR  CheckHandJointPos.J6.pz <50 THEN
				//不符合条件,剔除
				CONTINUE;			
			ELSE
				//判断SortType
				
				IF SortType =1 THEN 
					
					IF ABS(tmpJs.J1Gap)>IptAgMax OR ABS(tmpJs.J2Gap)>IptAgMax OR ABS(tmpJs.J3Gap)>IptAgMax OR ABS(tmpJs.J4Gap)>IptAgMax OR ABS(tmpJs.J5Gap)>IptAgMax OR ABS(tmpJs.J6Gap)>IptAgMax  THEN
						//不符合条件,剔除
						CONTINUE;						
					ELSE
						//符合条件,赋值
						InverseKinematics.IsNull := FALSE;
						
					END_IF	
				ELSE
					//符合条件,赋值
					InverseKinematics.IsNull := FALSE;
				END_IF
					
			END_IF
		
			//关节移动量最小
			IF tmp1 > tmp2 THEN
				tmp1 := tmp2;
				InverseKinematics.HandJoint := tmpJs;
			END_IF
		
		END_FOR

	END_IF

ELSE
	//tmpJointStruLen 为0 ,没有解
	InverseKinematics.IsNull := TRUE;
						
END_IF





        3.3.3主程序

//作者:AlongWu

PROGRAM PLC_PRG
VAR


Point1_R_Trig				:R_TRIG;			//point1按钮上升沿
Point2_R_Trig				:R_TRIG;			//point2按钮上升沿

Line_R_Trig					:R_TRIG;			//Line按钮上升沿
Ball_R_Trig					:R_TRIG;			//Ball按钮上升沿


HJA							:HandJointAg;
END_VAR


//按钮上升沿
Point1_R_Trig(CLK:=GVL.B_Point1Bt , Q=> );
Point2_R_Trig(CLK:=GVL.B_Point2Bt , Q=> );

Line_R_Trig(CLK:=GVL.B_LineBt , Q=> );
Ball_R_Trig(CLK:=GVL.B_BallBt , Q=> );


//point1运动触发
IF Point1_R_Trig.Q AND GVL.B_MotionAct = FALSE THEN
	
	GVL.LR_TargetPX := 100;
	GVL.LR_TargetPY := -100;
	GVL.LR_TargetPZ := 100;
	GVL.LR_TargetRX := 180;
	GVL.LR_TargetRY := 0;
	GVL.LR_TargetRZ := 0;

	GVL.MotionType := MotionType_Enum.Point;
	GVL.B_MotionAct := TRUE;
END_IF

//point2运动触发
IF Point2_R_Trig.Q AND GVL.B_MotionAct = FALSE THEN
	
	GVL.LR_TargetPX := 200;
	GVL.LR_TargetPY := -150;
	GVL.LR_TargetPZ := 150;
	GVL.LR_TargetRX := 90;
	GVL.LR_TargetRY := 90;
	GVL.LR_TargetRZ := 90;

	GVL.MotionType := MotionType_Enum.Point;
	GVL.B_MotionAct := TRUE;
END_IF



//line运动触发
IF Line_R_Trig.Q AND GVL.B_MotionAct = FALSE THEN
	
	GVL.LR_TargetPX := 100;
	GVL.LR_TargetPY := -100;
	GVL.LR_TargetPZ := 100;
	GVL.LR_TargetRX := 180;
	GVL.LR_TargetRY := 0;
	GVL.LR_TargetRZ := 0;


	GVL.MotionType := MotionType_Enum.Line;
	GVL.B_MotionAct := TRUE;
END_IF

//ball运动触发
IF Ball_R_Trig.Q AND GVL.B_MotionAct = FALSE THEN
	
		GVL.LR_Ball_R :=10;
		GVL.LR_Ball_Cx :=100;
		GVL.LR_Ball_Cy :=-200;
		GVL.LR_Ball_Cz :=200;


	GVL.MotionType := MotionType_Enum.Ball;
	GVL.B_MotionAct := TRUE;
END_IF


//计算所有关节的坐标
HJA.J1 := GVL.LR_Hand_J1;
HJA.J2 := GVL.LR_Hand_J2;
HJA.J3 := GVL.LR_Hand_J3;
HJA.J4 := GVL.LR_Hand_J4;
HJA.J5 := GVL.LR_Hand_J5;
HJA.J6 := GVL.LR_Hand_J6;

//正解,更新各关节的坐标
GVL.CurrentJointPos := DirectKinematics(HJointAg:= HJA);


        3.3.4运动主程序

//作者:AlongWu

PROGRAM Motion_PRG
VAR
	
i 	:INT;
Act_R_Trig					:R_TRIG;			//Act上升沿

END_VAR


Act_R_Trig(CLK:=GVL.B_MotionAct , Q=> );			//Act上升沿

IF Act_R_Trig.Q THEN
	//Act 上升沿,复位步骤和状态值
	
	GVL.I_PointM_Step:=0;
	GVL.I_LineM_Step:=0;
	GVL.I_BallM_Step:=0;
	GVL.B_InverseKineFalure :=FALSE;
END_IF


IF GVL.B_MotionAct THEN	
	CASE GVL.MotionType OF
		MotionType_Enum.Point:
		//启用 PointMotion的PRG
					PointMotion_PRG();
		MotionType_Enum.Line:
		//启用 LineMotion的PRG
					LineMotion_PRG();
		MotionType_Enum.Ball:
		//启用 BallMotion的PRG
					BallMotion_PRG();
		ELSE
			//motion结束
			GVL.B_MotionAct :=FALSE;
				
	END_CASE
END_IF

         3.3.4.1直线子运动程序

//作者:AlongWu

PROGRAM LineMotion_PRG
VAR
	


tmpPs				:PosStru;			//目标坐标数据元
tmpHandJR			:HandJointResult;	//逆解结果缓存

HandJR				:HandJointResult;	//逆解结果

LR_MoveLen			:LREAL;				//机械手末端移动距离 mm
LR_MovingTime		:LREAL;				//运动时长 s


tmp1				:LREAL;
tmp2				:LREAL;
tmp3				:LREAL;
tmp4				:LREAL;

ALen				:LREAL;				//加速距离长 mm
ATm					:LREAL;				//加速时长 s
SpdLowPos			:LREAL;				//减速位置 mm

Xg					:LREAL;
Yg					:LREAL;
Zg					:LREAL;

Xk					:LREAL;
Yk					:LREAL;
Zk					:LREAL;


Xb					:LREAL;
Yb					:LREAL;
Zb					:LREAL;




//6关节的 一元一次方程的 b 和 k。
j1b					:LREAL;
j1k					:LREAL;
j2b					:LREAL;
j2k					:LREAL;
j3b					:LREAL;
j3k					:LREAL;
j4b					:LREAL;
j4k					:LREAL;
j5b					:LREAL;
j5k					:LREAL;
j6b					:LREAL;
j6k					:LREAL;

slen				:LREAL;
v					:LREAL;
t					:LREAL;

END_VAR





CASE GVL.I_LineM_Step OF
	0:
		
		//坐标赋值
		tmpPs.px :=GVL.LR_TargetPX;
		tmpPs.py :=GVL.LR_TargetPY;
		tmpPs.pz :=GVL.LR_TargetPZ;
		tmpPs.rx :=GVL.LR_TargetRX;
		tmpPs.ry :=GVL.LR_TargetRY;
		tmpPs.rz :=GVL.LR_TargetRZ;
	
		GVL.I_LineM_Step := GVL.I_LineM_Step +1;

	1:
		//检查目标坐标是否有解
		//逆解
		tmpHandJR :=InverseKinematics(KeyPosStru:=tmpPs , SortType:=0 );
	
		IF tmpHandJR.IsNull THEN
			GVL.B_InverseKineFalure :=TRUE;
			GVL.I_LineM_Step := 99;			
			RETURN;
		ELSE
			GVL.I_LineM_Step :=GVL.I_LineM_Step + 1;
		
		END_IF
	2:
		//把角度摆好,再直线插补
		//坐标赋值
		tmpPs.px :=GVL.CurrentJointPos.J6.px;
		tmpPs.py :=GVL.CurrentJointPos.J6.py;
		tmpPs.pz :=GVL.CurrentJointPos.J6.pz;
		tmpPs.rx :=GVL.LR_TargetRX;
		tmpPs.ry :=GVL.LR_TargetRY;
		tmpPs.rz :=GVL.LR_TargetRZ;
	
		GVL.I_LineM_Step := GVL.I_LineM_Step +1;

	3:
			//逆解
		HandJR :=InverseKinematics(KeyPosStru:=tmpPs , SortType:=0 );
	
		IF HandJR.IsNull THEN
			GVL.B_InverseKineFalure :=TRUE;
			GVL.I_LineM_Step := 99;			
		ELSE
			GVL.I_LineM_Step :=GVL.I_LineM_Step + 1;
		END_IF	
	4:
		//按固定时长,机械手末端先移动至目标角度。
		LR_MovingTime := 5;
		
	 	j1b := GVL.LR_Hand_J1;
        j2b := GVL.LR_Hand_J2;
        j3b := GVL.LR_Hand_J3;
        j4b := GVL.LR_Hand_J4;
        j5b := GVL.LR_Hand_J5;
        j6b := GVL.LR_Hand_J6;

        j1k := HandJR.HandJoint.J1Gap / LR_MovingTime;
        j2k := HandJR.HandJoint.J2Gap / LR_MovingTime;
        j3k := HandJR.HandJoint.J3Gap / LR_MovingTime;
        j4k := HandJR.HandJoint.J4Gap / LR_MovingTime;
        j5k := HandJR.HandJoint.J5Gap / LR_MovingTime;
        j6k := HandJR.HandJoint.J6Gap / LR_MovingTime;
		
		t :=0;
		GVL.I_LineM_Step :=GVL.I_LineM_Step + 1;
	5:
		
		GVL.LR_Hand_J1 := j1b + j1k * t;	
		GVL.LR_Hand_J2 := j2b + j2k * t;
		GVL.LR_Hand_J3 := j3b + j3k * t;
		GVL.LR_Hand_J4 := j4b + j4k * t;
		GVL.LR_Hand_J5 := j5b + j5k * t;
		GVL.LR_Hand_J6 := j6b + j6k * t;
		t := t+ 0.05;
		IF t > LR_MovingTime THEN
			GVL.I_LineM_Step :=GVL.I_LineM_Step + 1;
		END_IF
	6:
		GVL.LR_Hand_J1:=HandJR.HandJoint.J1;
        GVL.LR_Hand_J2:=HandJR.HandJoint.J2;
        GVL.LR_Hand_J3:=HandJR.HandJoint.J3;
        GVL.LR_Hand_J4:=HandJR.HandJoint.J4;
        GVL.LR_Hand_J5:=HandJR.HandJoint.J5;
        GVL.LR_Hand_J6:=HandJR.HandJoint.J6;
		
		GVL.I_LineM_Step :=10;

	10:
		
		IF GVL.LR_MovingSpd = 0 OR GVL.LR_MovingAsp=0 THEN
			GVL.I_LineM_Step := 99;	
			RETURN;
		END_IF

		//计算当前坐标到目标坐标的距离和时长
		tmp1 :=GVL.LR_TargetPX - GVL.CurrentJointPos.J6.px;
		tmp2 :=GVL.LR_TargetPY - GVL.CurrentJointPos.J6.py;
		tmp3 :=GVL.LR_TargetPZ - GVL.CurrentJointPos.J6.pz;
	
		LR_MoveLen := SQRT(tmp1*tmp1+tmp2*tmp2+tmp3*tmp3);	
		
		ATm := GVL.LR_MovingSpd / GVL.LR_MovingAsp;
		
		ALen := GVL.LR_MovingAsp * ATm *ATm;
		
		IF ALen > LR_MoveLen THEN
			//未达到目标数独,已开始减速
			LR_MovingTime := SQRT(LR_MoveLen/GVL.LR_MovingAsp);
			SpdLowPos := LR_MoveLen * 0.5;
		ELSE
			LR_MovingTime :=ATm*2 + (LR_MoveLen-ALen)/GVL.LR_MovingSpd;
			SpdLowPos := LR_MoveLen - ALen*0.5;
		END_IF
			
		IF LR_MovingTime = 0 THEN
			GVL.I_LineM_Step := 99;	
			RETURN;
		END_IF
		
		//计算位移插补的一元一次方程参数, k 和 b。
		Xg := GVL.LR_TargetPX -GVL.CurrentJointPos.J6.px;
		Yg := GVL.LR_TargetPY -GVL.CurrentJointPos.J6.py;
		Zg := GVL.LR_TargetPZ -GVL.CurrentJointPos.J6.pz;
		
		
		Xk := Xg / LR_MoveLen;
		Yk := Yg / LR_MoveLen;
		Zk := Zg / LR_MoveLen;
		
		Xb := GVL.CurrentJointPos.J6.px;
		Yb := GVL.CurrentJointPos.J6.py;
		Zb := GVL.CurrentJointPos.J6.pz;
		
		slen :=0;
		v :=0;
		GVL.I_LineM_Step :=GVL.I_LineM_Step + 1;
	11:
		IF slen < LR_MoveLen THEN
			IF slen < SpdLowPos THEN
				IF v < GVL.LR_MovingSpd THEN
					//加速阶段
					v := v + GVL.LR_MovingAsp*0.05;
					slen := slen + v*0.05;
				ELSE
					//匀速阶段
					slen := slen + v*0.05;
				END_IF
			ELSE
				//减速阶段
				IF v<0.1 THEN
					v :=0.1;
				ELSE
					v := v - GVL.LR_MovingAsp*0.05;
				END_IF
				slen := slen + v*0.05;
			END_IF			
			
			tmpPs.px :=Xb + Xk*slen;
			tmpPs.py :=Yb + Yk*slen;
			tmpPs.pz :=Zb + Zk*slen;
		
			//逆解
			HandJR :=InverseKinematics(KeyPosStru:=tmpPs , SortType:=0 );
		
			IF HandJR.IsNull THEN
				GVL.B_InverseKineFalure :=TRUE;
				GVL.I_LineM_Step := 99;				
				RETURN;
			END_IF
			
			GVL.LR_Hand_J1:=HandJR.HandJoint.J1;
			GVL.LR_Hand_J2:=HandJR.HandJoint.J2;
			GVL.LR_Hand_J3:=HandJR.HandJoint.J3;
			GVL.LR_Hand_J4:=HandJR.HandJoint.J4;
			GVL.LR_Hand_J5:=HandJR.HandJoint.J5;
			GVL.LR_Hand_J6:=HandJR.HandJoint.J6;
							
			
		ELSE			
			GVL.I_LineM_Step :=GVL.I_LineM_Step + 1;					
		END_IF
	
	12:
		GVL.LR_Hand_J1:=tmpHandJR.HandJoint.J1;
		GVL.LR_Hand_J2:=tmpHandJR.HandJoint.J2;
		GVL.LR_Hand_J3:=tmpHandJR.HandJoint.J3;
		GVL.LR_Hand_J4:=tmpHandJR.HandJoint.J4;
		GVL.LR_Hand_J5:=tmpHandJR.HandJoint.J5;
		GVL.LR_Hand_J6:=tmpHandJR.HandJoint.J6;
			
		GVL.I_LineM_Step :=GVL.I_LineM_Step + 1;					
ELSE
		GVL.MotionType := 3;
END_CASE


        3.3.4.2阿基米德旋线+球面子运动程序

//作者:AlongWu

PROGRAM BallMotion_PRG
VAR
	
tmpPs				:PosStru;			//目标坐标数据元
tmpHandJR			:HandJointResult;	//逆解结果缓存

HandJR				:HandJointResult;	//逆解结果

LR_MoveLen			:LREAL;				//机械手末端移动距离 mm
LR_MovingTime		:LREAL;				//运动时长 s



BallR				:LREAL;
BallCx				:LREAL;
BallCy				:LREAL;
BallCz				:LREAL;



//6关节的 一元一次方程的 b 和 k。
j1b					:LREAL;
j1k					:LREAL;
j2b					:LREAL;
j2k					:LREAL;
j3b					:LREAL;
j3k					:LREAL;
j4b					:LREAL;
j4k					:LREAL;
j5b					:LREAL;
j5k					:LREAL;
j6b					:LREAL;
j6k					:LREAL;

t					:LREAL;
Arch_B				:LREAL;				//螺旋线间距系数
tmpR				:LREAL;				//当前直径长度 mm
ag					:LREAL;				//旋线的极角度
Z					:LREAL;				//当前Z位置 mm
v3d					:Vector3d;			//3维向量
eg					:EulerAngleStru;	//欧拉角组




END_VAR




  (*
    球的方程  
      x = x0+R*sinA*cosB
      y = y0+R*sinA*sinB
      z = z0+R*cosA
  
    阿基米德螺线方程
     
      r = a+bB;
   
    螺线距离 = 2* PI * b;
      
    本动作的目标是 用阿基米德螺线包住球。 同时 每个点的J6的z方向 指向 球心。
	
	
	 测试过程,运动过程,会出现 多个 非连续姿态(下一个姿态的1个或多个关节,的跨度过大。)
      
     测试用例:R=20, CX=100.CY=-200,CZ=100

  *)




CASE GVL.I_BallM_Step OF
	0:
		//参数赋值
		BallR :=GVL.LR_Ball_R;
		BallCx :=GVL.LR_Ball_Cx;
		BallCy :=GVL.LR_Ball_Cy;
		BallCz :=GVL.LR_Ball_Cz;	
		GVL.I_BallM_Step :=GVL.I_BallM_Step + 1;		
	1:
		//移动至球顶部,并调整姿态
		//坐标赋值
		tmpPs.px :=BallCx;
		tmpPs.py :=BallCy;
		tmpPs.pz :=BallCz + BallR;
		tmpPs.rx :=0;
		tmpPs.ry :=-180;
		tmpPs.rz :=-180;
	
		GVL.I_BallM_Step := GVL.I_BallM_Step +1;

	2:
			//逆解
		HandJR :=InverseKinematics(KeyPosStru:=tmpPs , SortType:=0 );
	
		IF HandJR.IsNull THEN
			GVL.B_InverseKineFalure :=TRUE;
			GVL.I_BallM_Step := 99;			
			RETURN;
		ELSE
			GVL.I_BallM_Step :=GVL.I_BallM_Step + 1;
		END_IF	
	3:
		//按固定时长,机械手末端先移动至目标角度。
		LR_MovingTime := 5;
		
	 	j1b := GVL.LR_Hand_J1;
        j2b := GVL.LR_Hand_J2;
        j3b := GVL.LR_Hand_J3;
        j4b := GVL.LR_Hand_J4;
        j5b := GVL.LR_Hand_J5;
        j6b := GVL.LR_Hand_J6;

        j1k := HandJR.HandJoint.J1Gap / LR_MovingTime;
        j2k := HandJR.HandJoint.J2Gap / LR_MovingTime;
        j3k := HandJR.HandJoint.J3Gap / LR_MovingTime;
        j4k := HandJR.HandJoint.J4Gap / LR_MovingTime;
        j5k := HandJR.HandJoint.J5Gap / LR_MovingTime;
        j6k := HandJR.HandJoint.J6Gap / LR_MovingTime;
		
		t :=0;
		GVL.I_BallM_Step :=GVL.I_BallM_Step + 1;
	4:
		
		GVL.LR_Hand_J1 := j1b + j1k * t;	
		GVL.LR_Hand_J2 := j2b + j2k * t;
		GVL.LR_Hand_J3 := j3b + j3k * t;
		GVL.LR_Hand_J4 := j4b + j4k * t;
		GVL.LR_Hand_J5 := j5b + j5k * t;
		GVL.LR_Hand_J6 := j6b + j6k * t;
		t := t+ 0.05;
		IF t > LR_MovingTime THEN
			GVL.I_BallM_Step :=GVL.I_BallM_Step + 1;
		END_IF
	5:
		GVL.LR_Hand_J1:=HandJR.HandJoint.J1;
        GVL.LR_Hand_J2:=HandJR.HandJoint.J2;
        GVL.LR_Hand_J3:=HandJR.HandJoint.J3;
        GVL.LR_Hand_J4:=HandJR.HandJoint.J4;
        GVL.LR_Hand_J5:=HandJR.HandJoint.J5;
        GVL.LR_Hand_J6:=HandJR.HandJoint.J6;
		
		GVL.I_BallM_Step :=10;

	10:
		Arch_B :=(BallR / 360) /3;				//螺旋线间距系数,3圈直径为目标直径
		tmpR :=0;
		ag :=0.1;
		GVL.I_BallM_Step :=GVL.I_BallM_Step + 1;
	11:
		IF tmpR < BallR THEN
			
			// 计算旋线的半径
			tmpR := Arch_B * ag;
			
			// 计算当前球面的坐标
			tmpPs.px :=BallCx + tmpR*CosAg(ag);
			tmpPs.py :=BallCy + tmpR*SinAg(ag);
			Z := BallCz + BallR - tmpR;
			tmpPs.pz := Z;
			
			//计算当前的机械手末端指向的向量, 从球面指向球心
			v3d.px := BallCx - tmpPs.px;				
			v3d.py := BallCy - tmpPs.py;				
			v3d.pz := BallCz - tmpPs.pz;				
			
			//计算当前的机械手末端指向的欧拉角
			eg := v3d2Euler(v3d);

			tmpPs.rx := eg.rx;
			tmpPs.ry := eg.ry;
			tmpPs.rz := eg.rz;
			
			

			//逆解各关节角度
			HandJR :=InverseKinematics(KeyPosStru:=tmpPs , SortType:=1 );
	
			IF HandJR.IsNull THEN
				GVL.B_InverseKineFalure :=TRUE;
				GVL.I_BallM_Step := 99;			
				RETURN;
			END_IF	
			
			GVL.LR_Hand_J1:=HandJR.HandJoint.J1;
			GVL.LR_Hand_J2:=HandJR.HandJoint.J2;
			GVL.LR_Hand_J3:=HandJR.HandJoint.J3;
			GVL.LR_Hand_J4:=HandJR.HandJoint.J4;
			GVL.LR_Hand_J5:=HandJR.HandJoint.J5;
			GVL.LR_Hand_J6:=HandJR.HandJoint.J6;
			
			//插补,增加角度
			ag :=ag + 0.3;
		ELSE
			GVL.I_BallM_Step :=GVL.I_BallM_Step + 1;	
		END_IF
	
	
ELSE
	GVL.MotionType := 3;
END_CASE

三、总结

        整个程序比较简单,没有用到特殊的函数或者功能块,只用了四则运算来裸写相关的计算。轴控制就不贴代码了,也没啥特点。只调用了MC_Power和SMC_ControlAxisByPos。由于机械手关节的电机控制是位置控制模式,只要把相应位置数值给到fSetPosition,轴即会根据速度参数自己转到位。

你可能感兴趣的:(codesys,6轴机械手,物联网,制造,机器人)