| 纯手工Codesys的6轴机械手控制程序记录 | 您所在的位置：网站首页 › cnc程序编写顺序教程 › 纯手工Codesys的6轴机械手控制程序记录 |
纯手工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 截图
程序主要分了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 OR tmp11 OR tmp3=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 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 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 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，轴即会根据速度参数自己转到位。 |
【本文地址】
公司简介
联系我们