Codesys的G代码控制Delta并联机械手

目录

一、概述

二、程序架构

三、关键程序代码

3.1 Prg_Plc

3.2 Prg_Motion

 3.3 Prg_Visu

一、概述

本文分享Codesys利用G代码控制Delta并联机械手的程序。Codesys有专门的功能块将CNC的插补动作转换至Robotics。具体可以查Codesys的帮助网站。

二、程序架构

三、关键程序代码

3.1 Prg_Plc

(*
#@Copyright:
#@License:
#@Birth:            created by AlongWu on 2023-06-17
#@Content:          Prg_Plc-主控制程序
#@Version:          0.0.1
#@Revision:         last revised by AlongWu on 2023-06-17
#@brief             
*)

PROGRAM Prg_Plc
VAR
	


arr_visu_3dpath_pointbuffer_bottom			:ARRAY[0..glvars.const_udi_size_points_in_array] OF VisuStruct3DPathPoint ;		//Visu的3D路径点位底画结构体数组


bw_start									:BOOL;								//程序启动位
bw_abort									:BOOL;								//程序终止
bs_running									:BOOL;								//程序运行中 状态
r_trig_start								:R_TRIG;							//程序启动上升沿
r_trig_abort								:R_TRIG;							//程序终止上升沿


arr_ncinterpreter_buf						:ARRAY[0..99] OF SMC_GEOINFO;		//SMC_NCInterpreter缓存空间

fb_readncfile2								:SMC_ReadNCFile2;					//G代码文件读取功能块 实例
fb_pathcopierfile							:SMC_PathCopierFile;				//G代码文件绑定Path3D控件的底画功能块  实例
fb_ncinterpreter							:SMC_NCInterpreter;					//G代码文件解析功能块  实例

i_step										:INT;								//步骤
i_counter									:INT;								//内部计时器



END_VAR


//程序启动上升沿	
r_trig_start(CLK:= NOT(bs_running) AND  bw_start, Q=> );

//程序终止上升沿
r_trig_abort(CLK:= bs_running AND  bw_abort, Q=> );

//程序启动上升沿触发后
IF r_trig_start.Q THEN
	
	//程序步跳至步骤1
	i_step := 1;
	
END_IF

//自复位
IF bw_start THEN
	
	//复位启动位
	bw_start := FALSE;
END_IF



//程序终止o上升沿触发后
IF r_trig_abort.Q THEN
	
	//程序步跳至步骤0
	i_step := 0;
	
	//复位运行状态位
	bs_running := FALSE;
	
	//功能块实例 复位
	fb_readncfile2.bExecute := FALSE;
	fb_pathcopierfile.bExecute := FALSE;
		
	
END_IF


//自复位
IF bw_abort THEN
	
	//复位启动位
	bw_abort := FALSE;
	
END_IF


//程序步骤
CASE i_step OF
	1:
		//功能块实例 复位
		fb_readncfile2.bExecute := FALSE;
		fb_pathcopierfile.bExecute := FALSE;
		
		//步骤+1
		i_step := i_step +1 ;
		
		//计时器复位
		i_counter := 0;
		
	2:
		//计数3个周期
		IF i_counter < 3 THEN
			i_counter := i_counter + 1 ;
		ELSE
			i_step := i_step +1 ;
		END_IF
	3:
		//功能块实例置位
		fb_readncfile2.bExecute := TRUE;
		fb_pathcopierfile.bExecute := TRUE;
		bs_running := TRUE;
		i_step := i_step +1 ;
	
ELSE
	//无操作
END_CASE


//调用子程序 Act_PathInit
Act_PathInit();
	


	

(*
#@Copyright:
#@License:
#@Birth:            created by Along on 2023-06-17
#@Content:          Act_PathInit-读取G代码的子程序
#@Version:          0.0.1
#@Revision:         last revised by Along on 2023-06-17
#@brief             
					功能块
					fb_readncfile2
					fb_ncinterpreter
					fb_checkvelocities
					fb_pathcopierfile
*)


//G代码文件读取功能块
fb_readncfile2(
	bExecute:= , 
	sFileName:= glvars.str_filepath, 
	pvl:= , 
	fDefaultVel:= 20, 
	fDefaultAccel:= 20, 
	fDefaultDecel:= 20, 
	fDefaultVelFF:= 20, 
	fDefaultAccelFF:= 20, 
	fDefaultDecelFF:= 20, 
	b3DMode:= TRUE , 
	bStepSuppress:= , 
	aSubProgramDirs:= , 
	bParenthesesAsComments:= , 
	bDisableJumpBuffer:= , 
	bBusy=> , 
	bError=> , 
	ErrorID=> , 
	errorPos=> , 
	ErrorProgramName=> , 
	sentences=> , 
	adwFileSize=> , 
	adwPos=> );

//G代码文件解析功能块  实例
fb_ncinterpreter(
	sentences:= fb_readncfile2.sentences, 
	bExecute:= fb_readncfile2.bExecute, 
	bAbort:= , 
	bAppend:= , 
	piStartPosition:= , 
	vStartToolLength:= , 
	nSizeOutQueue:= SIZEOF(arr_ncinterpreter_buf), 
	pbyBufferOutQueue:= ADR(arr_ncinterpreter_buf), 
	bEnableSyntaxChecks:= , 
	eOriConv:= , 
	dCircleTolerance:= , 
	pInterpreterStack:= , 
	nInterpreterStackSizeBytes:= , 
	bDone=> , 
	bBusy=> , 
	bError=> , 
	wErrorID=> , 
	errorPos=> , 
	poqDataOut=> , 
	iStatus=> , 
	iLineNumberDecoded=> , 
	GCodeText=> , 
	CallstackInfo=> , 
	aActivePrograms=> );
	
	
//G代码动作速度检查功能块  实例
glvars.fb_checkvelocities(
	bExecute:= fb_ncinterpreter.bDone, 
	bAbort:= , 
	poqDataIn:= fb_ncinterpreter.poqDataOut, 
	dAngleTol:= , 
	bCheckAddAxVelJump:= , 
	dMaxAddAxVelDifference:= , 
	bBusy=>, 
	bError=> , 
	wErrorID=> , 
	poqDataOut=>);
	
	

//G代码文件绑定Path3D控件的底画功能块  实例
fb_pathcopierfile(
	bExecute:= , 
	udiNumberOfPointsInArray:= glvars.const_udi_size_points_in_array, 
	pBuffer:= ADR(arr_visu_3dpath_pointbuffer_bottom) , 
	sFileName:= glvars.str_filepath, 
	pvl:= , 
	piStartPosition:= , 
	aSubProgramDirs:= , 
	bParenthesesAsComments:= , 
	bEnableSyntaxChecks:= , 
	bStepSuppress:= , 
	eOriConv:= , 
	bDone=> , 
	bError=> , 
	iErrorID=> , 
	vs3dt=> );
	
	

3.2 Prg_Motion

(*
#@Copyright:
#@License:
#@Birth:            created by AlongWu on 2023-06-17
#@Content:          Prg_Motion-运动控制程序
#@Version:          0.0.1
#@Revision:         last revised by AlongWu on 2023-06-17
#@brief             
*)

PROGRAM Prg_Motion
VAR
	

END_VAR



//G代码动作速度检查功能块  实例	
glvars.fb_interpolator(
	bExecute:= , 
	//插补器 导入 nc文件的outqueue
	poqDataIn:= glvars.fb_checkvelocities.poqDataOut, 
	bSlow_Stop:= , 
	bEmergency_Stop:= , 
	bWaitAtNextStop:= , 
	dOverride:= 1, 
	iVelMode:= , 
	dwIpoTime:=2000 , 
	dLastWayPos:= , 
	bAbort:= , 
	bSingleStep:= , 
	bAcknM:= , 
	bQuick_Stop:= , 
	dQuickDeceleration:= , 
	dJerkMax:= , 
	dQuickStopJerk:= , 
	bSuppressSystemMFunctions:= , 
	bDone=> , 
	bBusy=> , 
	bError=> , 
	wErrorID=> , 
	piSetPosition=> , 
	iStatus=> , 
	bWorking=> , 
	iActObjectSourceNo=> , 
	dActObjectLength=> , 
	dActObjectLengthRemaining=> , 
	dVel=> , 
	vecActTangent=> , 
	iLastSwitch=> , 
	dwSwitches=> , 
	dWayPos=> , 
	wM=> , 
	adToolLength=> , 
	Act_Object=> );
	

//Tripod并联机械手的移动控制功能块  实例
glvars.fb_trafo_tripod_Arm(
	pi:= glvars.fb_interpolator.piSetPosition, 
	dArmLength1:= glvars.const_lr_driven_armlen, 
	dArmLength2:= glvars.const_lr_passive_armlen, 
	dArm1Radius:= glvars.const_lr_driven_arm_r, 
	dStewartRadius:= glvars.const_lr_endplate_r, 
	dDistance:= glvars.const_lr_platejoin_dist, 
	dRotationOffset:= 0, 
	dOffsetA:= 0, 
	dOffsetB:= 0, 
	dOffsetC:= 0, 
	dMaxAngleBallJoint:= 60, 
	bError=> , 
	dA=> , 
	dB=> , 
	dC=> );


	
//绗架三维空间移动控制功能块  实例	
glvars.fb_trafo_gantry3d(
	pi:= glvars.fb_interpolator.piSetPosition,  
	dOffsetX:= , 
	dOffsetY:= , 
	dOffsetZ:= , 
	dx=> , 
	dy=> , 
	dz=> );	
	

 3.3 Prg_Visu

(*
#@Copyright:
#@License:
#@Birth:            created by AlongWu on 2023-06-17
#@Content:          Prg_Visu-可视化控制程序
#@Version:          0.0.1
#@Revision:         last revised by AlongWu on 2023-06-17
#@brief             
*)
PROGRAM Prg_Visu
VAR
	
fb_positiontracker							:SMC_PositionTracker;				//Visu的Path3D控件绑定路径跟踪功能块 实例
fb_trafof_tripod_Arm						:SMC_TRAFOF_Tripod_Arm;				//Tripod并联机械手Visu的绑定功能块 实例
fb_trafof_gantry3d							:SMC_TRAFOF_Gantry3D;				//绗架三维空间移Visu的绑定功能块 实例

arr_visu_3dpath_pointbuffer_track			:ARRAY[0..glvars.const_udi_size_points_in_array] OF VisuStruct3DPathPoint ;		//Visu的3D路径点位跟踪画结构体数组		

v3d_target									:SMC_VECTOR3D;						//末端三维坐标

END_VAR




//Visu的Path3D控件绑定路径跟踪功能块	
fb_positiontracker(
	bEnable:= glvars.fb_interpolator.bBusy, 
	bClear:= , 
	dX:= Virtual_X.diActPosition, 
	dY:= Virtual_Y.diActPosition, 
	dZ:= Virtual_Z.diActPosition, 
	udiNumberOfPointsInArray:= glvars.const_udi_size_points_in_array, 
	pBuffer:= ADR(arr_visu_3dpath_pointbuffer_track), 
	vs3dt=> );
	
	
//绗架三维空间移动控制功能块  实例
fb_trafof_tripod_Arm(
	dArmLength1:= glvars.const_lr_driven_armlen, 
	dArmLength2:= glvars.const_lr_passive_armlen, 
	dArm1Radius:= glvars.const_lr_driven_arm_r, 
	dStewartRadius:= glvars.const_lr_endplate_r, 
	dDistance:= glvars.const_lr_platejoin_dist, 
	dRotationOffset:= 0, 
	dOffsetA:= 0, 
	dOffsetB:= 0, 
	dOffsetC:= 0, 
	dx=> v3d_target.dX, 
	dy=> v3d_target.dY, 
	dz=> v3d_target.dZ, 
	DriveA:= Virtual_A1, 
	DriveB:= Virtual_A2, 
	DriveC:= Virtual_A3);	
	
//绗架三维空间移Visu的绑定功能块 实例	
fb_trafof_gantry3d(
	dOffsetX:= , 
	dOffsetY:= , 
	dOffsetZ:= , 
	minX:= , 
	maxX:= 200, 
	minY:= , 
	maxY:= 200, 
	minZ:= , 
	maxZ:= 200, 
	DriveX:= Virtual_X, 
	DriveY:= Virtual_Y, 
	DriveZ:=Virtual_Z , 
	dx=> , 
	dy=> , 
	dz=> , 
	dnx=> , 
	dny=> , 
	dnz=> , 
	dm=> );

 以上。